def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature] cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() filtered = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) if point.point.x <= max_distance: filtered.append(feature) return filtered
def feature_depths(features): cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() depths = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z])) return depths
def transform_pose(_pose2D, src_frame='CameraTop_frame', dst_frame='/base_footprint', timeout=3): tl = TransformListener() pose_stamped = PoseStamped() pose_stamped.header.stamp = rospy.Time() pose_stamped.header.frame_id = src_frame pose_stamped.pose = Pose(Point(_pose2D.x, _pose2D.y, 0.0), Quaternion()) try: tl.waitForTransform(target_frame=dst_frame, source_frame=src_frame, time=rospy.Time(), timeout=rospy.Duration(timeout)) pose_transf = tl.transformPose(dst_frame, pose_stamped) except Exception as e: rospy.logwarn("Transformation failed!!! %s" % str(e)) return _pose2D return Pose2D(pose_transf.pose.position.x, pose_transf.pose.position.y, 0.0)
def select_center(features): # type: (List[Feature]) -> Feature cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() angles = [] center_ray = np.array([1.0, 0.0, 0.0]) for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) ray = np.array([point.point.x, point.point.y, point.point.z]) ray /= np.linalg.norm(ray) angle = np.arccos(np.dot(center_ray, ray)) angles.append(abs(angle)) return features[np.argmin(angles)]
def transform_pose(pose, src_frame, dst_frame, timeout=10): """ Transforms the given pose from src_frame to dst_frame. :param src_frame :param dst_frame :param timeout the amount of time allowed (in secs) for a transformation (default 3) """ if str(type(pose)) != str(type(Pose())): rospy.logwarn(colors.BACKGROUND_RED + "The 'pose' should be a Pose object, not '%s'.%s" % (str(type(pose)).split('\'')[1], colors.NATIVE_COLOR)) from tf.listener import TransformListener assert timeout >= 1 pose_stamped = PoseStamped() pose_stamped.header.stamp = rospy.Time() pose_stamped.header.frame_id = src_frame pose_stamped.pose = pose global _tl if not _tl: _tl = TransformListener() rospy.sleep(0.5) timeout -= 0.5 rospy.loginfo("Transforming position from %s to %s coordinates..." % ( src_frame, dst_frame)) # If something fails we'll return the original pose (for testing # with mocks when tf isn't available) result = pose try: _tl.waitForTransform( target_frame=dst_frame, source_frame=src_frame, time=rospy.Time(), timeout=rospy.Duration(timeout)) pose_transf = _tl.transformPose(dst_frame, pose_stamped) result = pose_transf.pose except Exception as e: rospy.logwarn(colors.BACKGROUND_RED + "Warning! Pose transformation failed! %s%s" % (str(e), colors.NATIVE_COLOR)) return result
class abbRobot: errorDict = { 0: "Successful", -1: "Invalid goal", -2: "Invalid joints", -3: "Old header timestamp", -4: "Path tolerance violated", -5: "Goal tolerance violated" } def __init__(self): self.manip = mic.MoveGroupCommander("manipulator") self.client = act.SimpleActionClient('joint_trajectory_action', trajAction) rp.loginfo("Waiting for server joint_trajectory_action.") self.client.wait_for_server() rp.loginfo("Successfuly connected to server.") self.tfListener = TransformListener() def getEEPoint(self, start_link='base_link', end_link="tool0"): self.tfListener.waitForTransform(start_link, end_link, rp.Time(), rp.Duration(0.5)) ptData = self.tfListener.lookupTransform(start_link, end_link, rp.Time()) pts = [round(pt, 5) for pt in ptData[0]] orient = [round(pt, 5) for pt in ptData[1]] return pts, orient def move2Point(self, points, eAngles=[[0, 0, 0]], ax='sxyz', end_effector='link_6'): """ Moves to a point using end effector point space.\n Usage:\n - points - list of lists that contain x,y,z coordinates\n - eAngles - default [[0,0,0]]\n - list of lists that contain a,b,g euler angles - if multiple points are passed and only one list of euler angles then those angles are going to be used for all points - ax - specify convention to use for euler angles - default: 'sxyz' - end_effector - default link_6 - link whose point you want to move """ print "Number of points recieved: ", len(points) t1 = t.time() if (len(points) == len(eAngles) or len(eAngles) == 1): for i in xrange(len(points)): if rp.is_shutdown(): print "ROS has been shutdown. Exiting..." break print i + 1, ". point:", [round(pt, 5) for pt in points[i]] p = Point(points[i][0], points[i][1], points[i][2]) index = i if len(eAngles) == 1: index = 0 orient = Quaternion(*qEuler(eAngles[index][0], eAngles[index] [1], eAngles[index][2], ax)) self.manip.set_pose_target(Pose(p, orient), end_effector_link=end_effector) self.manip.go(True) rp.loginfo("Moving to multiple points finished.") self.__displayDuration(t1, t.time()) else: print "Number of points recieved does not match number of euler angles received\nneither number of euler angles is 1. Please check the input parameters." def cartesian2Point(self, points, eAngles, ax='sxyz', resolution=0.01, jumpStep=0, end_effector='link_6'): """ Moves to a point in a straight line using end effector point space.\n Usage:\n - points - list of lists that contain x,y,z coordinates\n - eAngles - list that contains a,b,g euler angles for constraint - ax - specify convention to use for euler angles - default: 'sxyz' - resolution - maximum distance between 2 generated points - default: 0.01 - jumpStep - maximum jump distance between 2 generated points - default: 0 - end_effector - link whose point you want to move - default link_6 """ print "Number of points recieved: ", len(points) wpose = Pose() self.manip.set_end_effector_link(end_effector) constraint = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.link_name = end_effector orientation_constraint.orientation = Quaternion( *qEuler(eAngles[0], eAngles[1], eAngles[2], ax)) constraint.orientation_constraints.append(orientation_constraint) self.manip.set_path_constraints(constraint) t1 = t.time() for pt in points: if not rp.is_shutdown(): print "Going to point: ", [round(p, 5) for p in pt] wpose.position.x = pt[0] wpose.position.y = pt[1] wpose.position.z = pt[2] (plan, factor) = self.manip.compute_cartesian_path([wpose], resolution, jumpStep) trajLen = len(plan.joint_trajectory.points) if trajLen <= 100 and factor == 1.0: self.manip.execute(plan) else: rp.loginfo( "Error while planning. No execution attempted.\nNo. points planned:{}\nPercentage of path found:{}%" .format(trajLen, round(factor * 100, 2))) rp.loginfo("Moving to multiple points finished.") self.__displayDuration(t1, t.time()) self.manip.set_path_constraints(None) def jointAction(self, jointAngles): """ Moves to a point using joint space\n Usage:\n - jointAngles - list of lists that contain 6 angles in order from joint_1 to joint_6 """ print "Number of joint angles recieved: ", len(jointAngles) t1 = t.time() jointGoal = goal() jointGoal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] jointGoal.trajectory.points.append(JointTrajectoryPoint()) for joint in jointAngles: print "Going to joint values[degrees]: ", [ round(i * 180 / pi, 2) for i in joint ] jointGoal.trajectory.points[0].positions = joint # jointGoal.trajectory.points[0].velocities=[2.,2.,2.,6.,6.,7.] # jointGoal.trajectory.points[0].accelerations=[1.,1.,1.,1.,1.,1.] # jointGoal.trajectory.points[0].time_from_start=rp.Duration(2,0) self.client.send_goal(jointGoal, feedback_cb=self.__feedback) self.client.wait_for_result() print "[Result:] ", self.errorDict[ self.client.get_result().error_code] spent = self.__displayDuration(t1, t.time()) @staticmethod def __feedback(msg): print "[Feedback:] ", msg.error @staticmethod def __displayDuration(t1, t2): spent = [(int(t2 - t1) / 60), 0, 0] spent[1] = int((t2 - t1) - spent[0] * 60) spent[2] = int(((t2 - t1) - spent[0] * 60 - spent[1]) * 1000) rp.loginfo("Execution took [min:sec.ms]: %i:%02i.%03i ", spent[0], spent[1], spent[2])
class Arm(object): """Arm controls the robot's arm. Joint space control: joints = ArmJoints() # Fill out joint states arm = fetch_api.Arm() arm.move_to_joints(joints) """ def __init__(self): self._joint_client = actionlib.SimpleActionClient( JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction) self._joint_client.wait_for_server(rospy.Duration(10)) self._move_group_client = actionlib.SimpleActionClient( MOVE_GROUP_ACTION_SERVER, MoveGroupAction) self._move_group_client.wait_for_server(rospy.Duration(10)) self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) self._tf_listener = TransformListener() def move_to_joints(self, joint_state): goal = control_msgs.msg.FollowJointTrajectoryGoal() goal.trajectory.joint_names.extend(ArmJoints.names()) point = trajectory_msgs.msg.JointTrajectoryPoint() point.positions.extend(joint_state.values()) point.time_from_start = rospy.Duration(TIME_FROM_START) goal.trajectory.points.append(point) self._joint_client.send_goal(goal) self._joint_client.wait_for_result(rospy.Duration(10)) def move_to_joint_goal(self, joints, allowed_planning_time=10.0, execution_timeout=rospy.Duration(15.0), group_name='arm', num_planning_attempts=1, plan_only=False, replan=False, replan_attempts=5, tolerance=0.01): """Moves the end-effector to a pose, using motion planning. Args: joints: A list of (name, value) for the arm joints. allowed_planning_time: float. The maximum duration to wait for a planning result. execution_timeout: float. The maximum duration to wait for an arm motion to execute (or for planning to fail completely), in seconds. group_name: string. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int. The number of times to compute the same plan. The shortest path is ultimately used. For random planners, this can help get shorter, less weird paths. plan_only: bool. If True, then this method does not execute the plan on the robot. Useful for determining whether this is likely to succeed. replan: bool. If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. replan_attempts: int. How many times to replan if the execution fails. tolerance: float. The goal tolerance, in meters. Returns: string describing the error if an error occurred, else None. """ goal_builder = MoveItGoalBuilder() goal_builder.set_joint_goal(*zip(*joints)) goal_builder.allowed_planning_time = allowed_planning_time goal_builder.num_planning_attempts = num_planning_attempts goal_builder.plan_only = plan_only goal_builder.planner_id = '' goal_builder.replan = replan goal_builder.replan_attempts = replan_attempts goal_builder.tolerance = tolerance goal = goal_builder.build() if goal is not None: self._move_group_client.send_goal(goal) # success = self._move_group_client.wait_for_result( # rospy.Duration(execution_timeout)) success = self._move_group_client.wait_for_result( execution_timeout) if not success: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) result = self._move_group_client.get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: return None else: return moveit_error_string(result.error_code.val) else: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) def move_to_pose(self, pose_stamped, allowed_planning_time=10.0, execution_timeout=15.0, group_name='arm', num_planning_attempts=1, orientation_constraint=None, plan_only=False, replan=False, replan_attempts=5, tolerance=0.01): """Moves the end-effector to a pose, using motion planning. Args: pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. allowed_planning_time: float. The maximum duration to wait for a planning result. execution_timeout: float. The maximum duration to wait for an arm motion to execute (or for planning to fail completely), in seconds. group_name: string. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int. The number of times to compute the same plan. The shortest path is ultimately used. For random planners, this can help get shorter, less weird paths. orientation_constraint: moveit_msgs/OrientationConstraint. An orientation constraint for the entire path. plan_only: bool. If True, then this method does not execute the plan on the robot. Useful for determining whether this is likely to succeed. replan: bool. If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. replan_attempts: int. How many times to replan if the execution fails. tolerance: float. The goal tolerance, in meters. Returns: string describing the error if an error occurred, else None. """ goal_builder = MoveItGoalBuilder() goal_builder.set_pose_goal(pose_stamped) if orientation_constraint is not None: goal_builder.orientation_constraint = orientation_constraint goal_builder.allowed_planning_time = allowed_planning_time goal_builder.num_planning_attempts = num_planning_attempts goal_builder.plan_only = plan_only goal_builder.replan = replan goal_builder.replan_attempts = replan_attempts goal_builder.tolerance = tolerance goal = goal_builder.build() if goal is not None: self._move_group_client.send_goal(goal) self._move_group_client.wait_for_result( rospy.Duration(execution_timeout)) result = self._move_group_client.get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: return None else: return moveit_error_string(result.error_code.val) else: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) def move_to_pose_with_seed(self, pose_stamped, seed_state=None, trajectory_constraint=[], allowed_planning_time=10.0, execution_timeout=15.0, group_name='arm', num_planning_attempts=1, orientation_constraint=None, plan_only=False, replan=False, replan_attempts=5, tolerance=0.01): """Moves the end-effector to a pose, using motion planning. Args: pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. seed_state: sensor_msgs/JointState. The start joint state to search for solution. trajectory_constraint: array of moveit_msgs/Constraints. The trajectory constraint. allowed_planning_time: float. The maximum duration to wait for a planning result. execution_timeout: float. The maximum duration to wait for an arm motion to execute (or for planning to fail completely), in seconds. group_name: string. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int. The number of times to compute the same plan. The shortest path is ultimately used. For random planners, this can help get shorter, less weird paths. orientation_constraint: moveit_msgs/OrientationConstraint. An orientation constraint for the entire path. plan_only: bool. If True, then this method does not execute the plan on the robot. Useful for determining whether this is likely to succeed. replan: bool. If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. replan_attempts: int. How many times to replan if the execution fails. tolerance: float. The goal tolerance, in meters. Returns: string describing the error if an error occurred, else None. """ goal_builder = MoveItGoalBuilder() goal_builder.set_pose_goal(pose_stamped) if orientation_constraint is not None: goal_builder.add_path_orientation_constraint( orientation_constraint) goal_builder.allowed_planning_time = allowed_planning_time goal_builder.num_planning_attempts = num_planning_attempts goal_builder.plan_only = plan_only goal_builder.replan = replan goal_builder.replan_attempts = replan_attempts goal_builder.tolerance = tolerance if seed_state: goal_builder.start_state.joint_state = seed_state if trajectory_constraint: goal_builder.add_trajectory_orientation_constraint( trajectory_constraint) goal = goal_builder.build() if goal is not None: self._move_group_client.send_goal(goal) self._move_group_client.wait_for_result( rospy.Duration(execution_timeout)) result = self._move_group_client.get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: return None else: return moveit_error_string(result.error_code.val) else: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) def straight_move_to_pose(self, group, plan): """Moves the end-effector to a pose in a straight line. Args: group: moveit_commander.MoveGroupCommander. The planning group for the arm. plan: the plan for the arm to follow Returns: string describing the error if an error occurred, else None. """ # Execute path result = group.execute(plan, wait=True) if not result: return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN) else: return None def straight_move_to_pose_check(self, group, pose_stamped, ee_step=0.025, jump_threshold=2.0, avoid_collisions=True): """Checks if we can move the end-effector to a pose in a straight line. Args: group: moveit_commander.MoveGroupCommander. The planning group for the arm. pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. ee_step: float. The distance in meters to interpolate the path. jump_threshold: float. The maximum allowable distance in the arm's configuration space allowed between two poses in the path. Used to prevent "jumps" in the IK solution. avoid_collisions: bool. Whether to check for obstacles or not. Returns: the generated plan if the arm can move ina straight line, else None. """ # Transform pose into planning frame self._tf_listener.waitForTransform(pose_stamped.header.frame_id, group.get_planning_frame(), rospy.Time.now(), rospy.Duration(1.0)) try: pose_transformed = self._tf_listener.transformPose( group.get_planning_frame(), pose_stamped) except (tf.LookupException, tf.ConnectivityException): rospy.logerr('Unable to transform pose from frame {} to {}'.format( pose_stamped.header.frame_id, group.get_planning_frame())) return moveit_error_string( MoveItErrorCodes.FRAME_TRANSFORM_FAILURE) # Compute path plan, fraction = group.compute_cartesian_path( [group.get_current_pose().pose, pose_transformed.pose], ee_step, jump_threshold, avoid_collisions) if fraction < 1: return None else: return plan def check_pose(self, pose_stamped, allowed_planning_time=10.0, group_name='arm', tolerance=0.01): return self.move_to_pose(pose_stamped, allowed_planning_time=allowed_planning_time, group_name=group_name, tolerance=tolerance, plan_only=True) def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): """Computes inverse kinematics for the given pose. Note: if you are interested in returning the IK solutions, we have shown how to access them. Args: pose_stamped: geometry_msgs/PoseStamped. timeout: rospy.Duration. How long to wait before giving up on the IK solution. Returns: A list of (name, value) for the arm joints if the IK solution was found, False otherwise. """ request = GetPositionIKRequest() request.ik_request.pose_stamped = pose_stamped request.ik_request.group_name = 'arm' request.ik_request.timeout = timeout response = self._compute_ik(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False joint_state = response.solution.joint_state joints = [] for name, position in zip(joint_state.name, joint_state.position): if name in ArmJoints.names(): joints.append((name, position)) return joints def compute_fk(self, joint_state): """Computes forward kinematics for the given joint state. Args: joint_state: sensor_msgs/JointState. Returns: A geometry_msgs/PoseStamped of the wrist position, False otherwise. """ request = GetPositionFKRequest() request.header.frame_id = 'base_link' request.fk_link_names = ['wrist_roll_link'] request.robot_state.joint_state = joint_state response = self._compute_fk(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False return response.pose_stamped def get_cartesian_path(self, group, start_joint_state, waypoints, ee_step=0.025, jump_threshold=2.0, avoid_collisions=True): """Returns a robot trajectory which passes through all the waypoints specified. Args: group: moveit_commander.MoveGroupCommander. The planning group for the arm. start_joint_state: sensor_msgs/JointState. waypoints: geometry_msgs/Pose[]. Waypoints to pass through. pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. ee_step: float. The distance in meters to interpolate the path. jump_threshold: float. The maximum allowable distance in the arm's configuration space allowed between two poses in the path. Used to prevent "jumps" in the IK solution. avoid_collisions: bool. Whether to check for obstacles or not. Returns: A RobotTrajectory to follow, False otherwise. """ # request = GetCartesianPathRequest() # request.header.frame_id = 'base_link' # request.start_state.joint_state = start_joint_state # request.group_name = ARM_GROUP_NAME # request.waypoints = waypoints # request.max_step = 0.025 # request.jump_threshold = 2.0 # request.avoid_collisions = True # response = self._get_cartesian_path(request) # error_str = moveit_error_string(response.error_code.val) # success = error_str == 'SUCCESS' # if not success: # return False # return response.solution # Compute path plan, fraction = group.compute_cartesian_path(waypoints, ee_step, jump_threshold, avoid_collisions) if fraction < 1: return None else: return plan def execute_trajectory(self, group, trajectory): """Moves the end-effector along the given trajectory. Args: group: moveit_commander.MoveGroupCommander. The planning group for the arm. trajectory: the RobotTrajectory to follow. Return: string describing the error if an error occurred, else None. """ result = group.execute(trajectory, wait=True) if not result: return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN) else: return None def cancel_all_goals(self): self._move_group_client.cancel_all_goals() self._joint_client.cancel_all_goals()
class DataCollectorAndLabeler: def __init__(self, output_folder, server, menu_handler, marker_size, calibration_file): if not os.path.exists(output_folder): os.mkdir(output_folder) # Create the new folder else: while True: msg = Fore.YELLOW + "To continue, the directory '{}' will be delete.\n" msg = msg + "Do you wish to continue? [y/N] " + Style.RESET_ALL answer = raw_input(msg.format(output_folder)) if len(answer) > 0 and answer[0].lower() in ('y', 'n'): if answer[0].lower() == 'n': sys.exit(1) else: break else: sys.exit(1) # defaults to N shutil.rmtree(output_folder) # Delete old folder os.mkdir(output_folder) # Recreate the folder self.output_folder = output_folder self.listener = TransformListener() self.sensors = {} self.sensor_labelers = {} self.server = server self.menu_handler = menu_handler self.data_stamp = 0 self.collections = {} self.bridge = CvBridge() self.config = loadJSONConfig(calibration_file) if self.config is None: sys.exit(1) # loadJSON should tell you why. self.world_link = self.config['world_link'] # Add sensors print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL) print('Number of sensors: ' + str(len(self.config['sensors']))) # Go through the sensors in the calib config. for sensor_key, value in self.config['sensors'].items(): # Create a dictionary that describes this sensor sensor_dict = {'_name': sensor_key, 'parent': value['link'], 'calibration_parent': value['parent_link'], 'calibration_child': value['child_link']} # TODO replace by utils function print("Waiting for message") msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg) connection_header = msg._connection_header['type'].split('/') ros_pkg = connection_header[0] + '.msg' msg_type = connection_header[1] print('Topic ' + value['topic_name'] + ' has type ' + msg_type) sensor_dict['topic'] = value['topic_name'] sensor_dict['msg_type'] = msg_type # If topic contains a message type then get a camera_info message to store along with the sensor data if sensor_dict['msg_type'] == 'Image': # if it is an image must get camera_info sensor_dict['camera_info_topic'] = os.path.dirname(sensor_dict['topic']) + '/camera_info' from sensor_msgs.msg import CameraInfo camera_info_msg = rospy.wait_for_message(sensor_dict['camera_info_topic'], CameraInfo) from rospy_message_converter import message_converter sensor_dict['camera_info'] = message_converter.convert_ros_message_to_dictionary(camera_info_msg) # Get the kinematic chain form world_link to this sensor's parent link now = rospy.Time() self.listener.waitForTransform(value['link'], self.world_link, now, rospy.Duration(5)) chain = self.listener.chain(value['link'], now, self.world_link, now, self.world_link) chain_list = [] for parent, child in zip(chain[0::], chain[1::]): key = self.generateKey(parent, child) chain_list.append({'key': key, 'parent': parent, 'child': child}) sensor_dict['chain'] = chain_list # Add to sensor dictionary self.sensors[sensor_key] = sensor_dict sensor_labeler = InteractiveDataLabeler(self.server, self.menu_handler, sensor_dict, marker_size, self.config['calibration_pattern']) self.sensor_labelers[sensor_key] = sensor_labeler print('finished visiting sensor ' + sensor_key) print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' + str(sensor_dict)) # print('sensor_labelers:') # print(self.sensor_labelers) self.abstract_transforms = self.getAllAbstractTransforms() # print("abstract_transforms = " + str(self.abstract_transforms)) def getTransforms(self, abstract_transforms, time=None): transforms_dict = {} # Initialize an empty dictionary that will store all the transforms for this data-stamp if time is None: time = rospy.Time.now() for ab in abstract_transforms: # Update all transformations self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0)) (trans, quat) = self.listener.lookupTransform(ab['parent'], ab['child'], time) key = self.generateKey(ab['parent'], ab['child']) transforms_dict[key] = {'trans': trans, 'quat': quat, 'parent': ab['parent'], 'child': ab['child']} return transforms_dict def lockAllLabelers(self): for sensor_name, sensor in self.sensors.iteritems(): self.sensor_labelers[sensor_name].lock.acquire() print("Locked all labelers") def unlockAllLabelers(self): for sensor_name, sensor in self.sensors.iteritems(): self.sensor_labelers[sensor_name].lock.release() print("Unlocked all labelers") def getLabelersTimeStatistics(self): stamps = [] # a list of the several time stamps of the stored messages for sensor_name, sensor in self.sensors.iteritems(): stamps.append(copy.deepcopy(self.sensor_labelers[sensor_name].msg.header.stamp)) max_delta = getMaxTimeDelta(stamps) average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs print('Times:') for stamp in stamps: printRosTime(stamp) return stamps, average_time, max_delta def saveCollection(self): # -------------------------------------- # Collect sensor data and labels (images, laser scans, etc) # -------------------------------------- # Lock the semaphore for all labelers self.lockAllLabelers() # Analyse message time stamps and decide if collection can be stored stamps, average_time, max_delta = self.getLabelersTimeStatistics() if max_delta.to_sec() > float(self.config['max_duration_between_msgs']): # times are close enough? rospy.logwarn('Max duration between msgs in collection is ' + str(max_delta.to_sec()) + '. Not saving collection.') self.unlockAllLabelers() return None else: # test passed rospy.loginfo('Max duration between msgs in collection is ' + str(max_delta.to_sec())) # Collect all the transforms transforms = self.getTransforms(self.abstract_transforms, average_time) # use average time of sensor msgs printRosTime(average_time, "Collected transforms for time ") all_sensor_data_dict = {} all_sensor_labels_dict = {} for sensor_name, sensor in self.sensors.iteritems(): print('collect sensor: ' + sensor_name) msg = copy.deepcopy(self.sensor_labelers[sensor_name].msg) labels = copy.deepcopy(self.sensor_labelers[sensor_name].labels) # TODO add exception also for point cloud and depth image # Update sensor data --------------------------------------------- if sensor['msg_type'] == 'Image': # Special case of requires saving image data as png separate files cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") # Convert to opencv image and save image to disk filename = self.output_folder + '/' + sensor['_name'] + '_' + str(self.data_stamp) + '.jpg' filename_relative = sensor['_name'] + '_' + str(self.data_stamp) + '.jpg' cv2.imwrite(filename, cv_image) image_dict = message_converter.convert_ros_message_to_dictionary(msg) # Convert sensor data to dictionary del image_dict['data'] # Remove data field (which contains the image), and replace by "data_file" image_dict['data_file'] = filename_relative # Contains full path to where the image was saved # Update the data dictionary for this data stamp all_sensor_data_dict[sensor['_name']] = image_dict else: # Update the data dictionary for this data stamp all_sensor_data_dict[sensor['_name']] = message_converter.convert_ros_message_to_dictionary(msg) # Update sensor labels --------------------------------------------- if sensor['msg_type'] in ['Image', 'LaserScan']: all_sensor_labels_dict[sensor_name] = labels else: raise ValueError('Unknown message type.') collection_dict = {'data': all_sensor_data_dict, 'labels': all_sensor_labels_dict, 'transforms': transforms} self.collections[self.data_stamp] = collection_dict self.data_stamp += 1 # Save to json file D = {'sensors': self.sensors, 'collections': self.collections, 'calibration_config': self.config} self.createJSONFile(self.output_folder + '/data_collected.json', D) self.unlockAllLabelers() def getAllAbstractTransforms(self): rospy.sleep(0.5) # wait for transformations # Get a list of all transforms to collect transforms_list = [] now = rospy.Time.now() all_frames = self.listener.getFrameStrings() for frame in all_frames: chain = self.listener.chain(frame, now, self.world_link, now, self.world_link) for idx in range(0, len(chain) - 1): parent = chain[idx] child = chain[idx + 1] transforms_list.append({'parent': parent, 'child': child, 'key': self.generateKey(parent, child)}) # https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique uniq_l = list(map(dict, frozenset(frozenset(i.items()) for i in transforms_list))) return uniq_l # get unique values def createJSONFile(self, output_file, D): print("Saving the json output file to " + str(output_file) + ", please wait, it could take a while ...") f = open(output_file, 'w') json.encoder.FLOAT_REPR = lambda f: ("%.6f" % f) # to get only four decimal places on the json file print >> f, json.dumps(D, indent=2, sort_keys=True) f.close() print("Completed.") @staticmethod def generateKey(parent, child, suffix=''): return parent + '-' + child + suffix
class MoveItGoalBuilder(object): """Builds a MoveGroupGoal. Example: # To do a reachability check from the current robot pose. builder = MoveItGoalBuilder() builder.set_pose_goal(pose_stamped) builder.allowed_planning_time = 5 builder.plan_only = True goal = builder.build() # To move to a current robot pose with a few options changed. builder = MoveItGoalBuilder() builder.set_pose_goal(pose_stamped) builder.replan = True builder.replan_attempts = 10 goal = builder.build() Here are the most common class attributes you might set before calling build(), and their default values: allowed_planning_time: float=10. How much time to allow the planner, in seconds. group_name: string='arm'. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int=1. How many times to compute the same plan (most planners are randomized). The shortest plan will be used. plan_only: bool=False. Whether to only compute the plan but not execute it. replan: bool=False. Whether to come up with a new plan if there was an error executing the original plan. replan_attempts: int=5. How many times to do replanning. replay_delay: float=1. How long to wait in between replanning, in seconds. tolerance: float=0.01. The tolerance, in meters for the goal pose. """ def __init__(self): self.allowed_planning_time = 10.0 self.fixed_frame = 'base_link' self.gripper_frame = 'wrist_roll_link' self.group_name = 'arm' self.planning_scene_diff = moveit_msgs.msg.PlanningScene() self.planning_scene_diff.is_diff = True self.planning_scene_diff.robot_state.is_diff = True self.look_around = False self.max_acceleration_scaling_factor = 0 self.max_velocity_scaling_factor = 0 self.num_planning_attempts = 1 self.plan_only = False self.planner_id = 'RRTConnectkConfigDefault' self.replan = False self.replan_attempts = 5 self.replan_delay = 1 self.start_state = moveit_msgs.msg.RobotState() self.start_state.is_diff = True self.tolerance = 0.01 self._orientation_contraints = [] self._pose_goal = None self._joint_names = None self._joint_positions = None self._tf_listener = TransformListener() def set_pose_goal(self, pose_stamped): """Sets a pose goal. Pose and joint goals are mutually exclusive. The most recently set goal wins. Args: pose_stamped: A geometry_msgs/PoseStamped. """ self._pose_goal = pose_stamped self._joint_names = None self._joint_positions = None def set_joint_goal(self, joint_names, joint_positions): """Set a joint-space planning goal. Args: joint_names: A list of strings. The names of the joints in the goal. joint_positions: A list of floats. The joint angles to achieve. """ self._joint_names = joint_names self._joint_positions = joint_positions self._pose_goal = None def add_path_orientation_contraint(self, o_constraint): """Adds an orientation constraint to the path. Args: o_constraint: A moveit_msgs/OrientationConstraint. """ self._orientation_contraints.append(copy.deepcopy(o_constraint)) self.planner_id = 'RRTConnectkConfigDefault' def build(self, tf_timeout=rospy.Duration(5.0)): """Builds the goal message. To set a pose or joint goal, call set_pose_goal or set_joint_goal before calling build. To add a path orientation constraint, call add_path_orientation_constraint first. Args: tf_timeout: rospy.Duration. How long to wait for a TF message. Only used with pose goals. Returns: moveit_msgs/MoveGroupGoal """ goal = MoveGroupGoal() # Set start state goal.request.start_state = copy.deepcopy(self.start_state) # Set goal constraint if self._pose_goal is not None: self._tf_listener.waitForTransform(self._pose_goal.header.frame_id, self.fixed_frame, rospy.Time.now(), tf_timeout) try: pose_transformed = self._tf_listener.transformPose( self.fixed_frame, self._pose_goal) except (tf.LookupException, tf.ConnectivityException): return None c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self.fixed_frame c1.position_constraints[0].link_name = self.gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [self.tolerance * self.tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(self._pose_goal.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self.fixed_frame c1.orientation_constraints[ 0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = self.gripper_frame c1.orientation_constraints[ 0].absolute_x_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_y_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_z_axis_tolerance = self.tolerance c1.orientation_constraints[0].weight = 1.0 goal.request.goal_constraints.append(c1) elif self._joint_names is not None: c1 = Constraints() for i in range(len(self._joint_names)): c1.joint_constraints.append(JointConstraint()) c1.joint_constraints[i].joint_name = self._joint_names[i] c1.joint_constraints[i].position = self._joint_positions[i] c1.joint_constraints[i].tolerance_above = self.tolerance c1.joint_constraints[i].tolerance_below = self.tolerance c1.joint_constraints[i].weight = 1.0 goal.request.goal_constraints.append(c1) # Set path constraints goal.request.path_constraints.orientation_constraints = self._orientation_contraints # Set trajectory constraints # Set planner ID (name of motion planner to use) goal.request.planner_id = self.planner_id # Set group name goal.request.group_name = self.group_name # Set planning attempts goal.request.num_planning_attempts = self.num_planning_attempts # Set planning time goal.request.allowed_planning_time = self.allowed_planning_time # Set scaling factors goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor # Set planning scene diff goal.planning_options.planning_scene_diff = copy.deepcopy( self.planning_scene_diff) # Set is plan only goal.planning_options.plan_only = self.plan_only # Set look around goal.planning_options.look_around = self.look_around # Set replanning options goal.planning_options.replan = self.replan goal.planning_options.replan_attempts = self.replan_attempts goal.planning_options.replan_delay = self.replan_delay return goal
class Sensor: def __init__(self, name, server, menu_handler, frame_world, frame_opt_parent, frame_opt_child, frame_sensor, marker_scale): print('Creating a new sensor named ' + name) self.name = name self.server = server self.menu_handler = menu_handler self.listener = TransformListener() self.br = tf.TransformBroadcaster() self.marker_scale = marker_scale # transforms self.world_link = frame_world self.opt_parent_link = frame_opt_parent self.opt_child_link = frame_opt_child self.sensor_link = frame_sensor self.updateAll() # update all the transformations # print('Collected pre, opt and pos transforms.') # # print('preT:\n' + str(self.preT)) # print('optT:\n' + str(self.optT)) # print('posT:\n' + str(self.posT)) self.optTInitial = copy.deepcopy(self.optT) self.createInteractiveMarker() # create interactive marker print('Created interactive marker.') # Start publishing now self.timer_callback = rospy.Timer( rospy.Duration(.1), self.publishTFCallback) # to periodically broadcast def resetToInitalPose(self): self.optT.matrix = self.optTInitial.matrix trans = self.optT.getTranslation() self.marker.pose.position.x = trans[0] self.marker.pose.position.y = trans[1] self.marker.pose.position.z = trans[2] quat = self.optT.getQuaternion() self.marker.pose.orientation.x = quat[0] self.marker.pose.orientation.y = quat[1] self.marker.pose.orientation.z = quat[2] self.marker.pose.orientation.w = quat[3] self.optTInitial = copy.deepcopy(self.optT) self.menu_handler.reApply(self.server) self.server.applyChanges() def publishTFCallback(self, _): trans = self.optT.getTranslation() quat = self.optT.getQuaternion() self.br.sendTransform((trans[0], trans[1], trans[2]), (quat[0], quat[1], quat[2], quat[3]), rospy.Time.now(), self.opt_child_link, self.opt_parent_link) def markerFeedback(self, feedback): # print(' sensor ' + self.name + ' received feedback') self.optT.setTranslationFromPosePosition(feedback.pose.position) self.optT.setQuaternionFromPoseQuaternion(feedback.pose.orientation) self.menu_handler.reApply(self.server) self.server.applyChanges() def updateAll(self): self.updatePreT() self.updateOptT() self.updatePosT() def updateOptT(self): self.optT = self.updateT(self.opt_parent_link, self.opt_child_link, rospy.Time.now()) def updatePreT(self): self.preT = self.updateT(self.world_link, self.opt_parent_link, rospy.Time.now()) def updatePosT(self): self.posT = self.updateT(self.opt_child_link, self.sensor_link, rospy.Time.now()) def updateT(self, parent_link, child_link, stamp): # self.listener.waitForTransform(parent_link, child_link, stamp, rospy.Duration(3.0)) # (trans, quat) = self.listener.lookupTransform(parent_link, child_link, stamp) self.listener.waitForTransform(parent_link, child_link, rospy.Time(), rospy.Duration(1.0)) (trans, quat) = self.listener.lookupTransform(parent_link, child_link, rospy.Time()) T = TransformationT(parent_link, child_link) T.setTranslation(trans) T.setQuaternion(quat) return T def createInteractiveMarker(self): self.marker = InteractiveMarker() self.marker.header.frame_id = self.opt_parent_link trans = self.optT.getTranslation() self.marker.pose.position.x = trans[0] self.marker.pose.position.y = trans[1] self.marker.pose.position.z = trans[2] quat = self.optT.getQuaternion() self.marker.pose.orientation.x = quat[0] self.marker.pose.orientation.y = quat[1] self.marker.pose.orientation.z = quat[2] self.marker.pose.orientation.w = quat[3] self.marker.scale = self.marker_scale self.marker.name = self.name self.marker.description = self.name + '_control' # insert a box control = InteractiveMarkerControl() control.always_visible = True marker_box = Marker() marker_box.type = Marker.SPHERE marker_box.scale.x = self.marker.scale * 0.3 marker_box.scale.y = self.marker.scale * 0.3 marker_box.scale.z = self.marker.scale * 0.3 marker_box.color.r = 0 marker_box.color.g = 1 marker_box.color.b = 0 marker_box.color.a = 0.2 control.markers.append(marker_box) self.marker.controls.append(control) self.marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) self.server.insert(self.marker, self.markerFeedback) self.menu_handler.apply(self.server, self.marker.name)
from tf.listener import TransformListener from tf.broadcaster import TransformBroadcaster from std_msgs.msg import Header import numpy as np from tf2_msgs.msg import TFMessage rospy.init_node('map_to_odom') listener = TransformListener() publisher = TransformBroadcaster() rate = rospy.Rate(100) rospy.sleep(5) time = rospy.Time.now() listener.waitForTransform(target_frame='camera_link_slam', source_frame='map', time=time, timeout=rospy.Duration(1)) listener.waitForTransform(target_frame='odom', source_frame='camera_link', time=time, timeout=rospy.Duration(1)) trans = None rot = None camera_link_slam_stamp = None camera_link_stamp = None def set_stamp(message): # type: (TFMessage) -> None global camera_link_slam_stamp, camera_link_stamp if message.transforms[0].child_frame_id == 'camera_link_slam':
class DataCollectorAndLabeler: def __init__(self, args, server, menu_handler): self.output_folder = utilities.resolvePath(args['output_folder']) if os.path.exists( self.output_folder ) and not args['overwrite']: # dataset path exists, abort print( '\n' + Fore.RED + 'Error: Dataset ' + self.output_folder + ' exists.\nIf you want to replace it add a "--overwrite" flag.' + Style.RESET_ALL + '\n') rospy.signal_shutdown() elif os.path.exists( self.output_folder ) and args['overwrite']: # move existing path to a backup location now = datetime.now() dt_string = now.strftime("%Y-%m-%d-%H-%M-%S") basename = os.path.basename(self.output_folder) backup_folder = '/tmp/' + basename + '_' + dt_string time.sleep(2) print('\n\nWarning: Dataset ' + Fore.YELLOW + self.output_folder + Style.RESET_ALL + ' exists.\nMoving it to a new folder: ' + Fore.YELLOW + backup_folder + '\nThis will be deleted after a system reboot!' + Style.RESET_ALL + '\n\n') time.sleep(2) execute('mv ' + self.output_folder + ' ' + backup_folder, verbose=True) os.mkdir(self.output_folder) # Recreate the folder self.listener = TransformListener() self.sensors = {} self.sensor_labelers = {} self.server = server self.menu_handler = menu_handler self.data_stamp = 0 self.collections = {} self.bridge = CvBridge() self.config = loadConfig(args['calibration_file']) if self.config is None: sys.exit(1) # loadJSON should tell you why. self.world_link = self.config['world_link'] # Add sensors print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL) print('Number of sensors: ' + str(len(self.config['sensors']))) # Go through the sensors in the calib config. for sensor_key, value in self.config['sensors'].items(): # Create a dictionary that describes this sensor sensor_dict = { '_name': sensor_key, 'parent': value['link'], 'calibration_parent': value['parent_link'], 'calibration_child': value['child_link'] } # TODO replace by utils function print("Waiting for message " + value['topic_name'] + ' ...') msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg) print('... received!') connection_header = msg._connection_header['type'].split('/') ros_pkg = connection_header[0] + '.msg' msg_type = connection_header[1] print('Topic ' + value['topic_name'] + ' has type ' + msg_type) sensor_dict['topic'] = value['topic_name'] sensor_dict['msg_type'] = msg_type # If topic contains a message type then get a camera_info message to store along with the sensor data if sensor_dict[ 'msg_type'] == 'Image': # if it is an image must get camera_info sensor_dict['camera_info_topic'] = os.path.dirname( sensor_dict['topic']) + '/camera_info' from sensor_msgs.msg import CameraInfo print('Waiting for camera_info message on topic ' + sensor_dict['camera_info_topic'] + ' ...') camera_info_msg = rospy.wait_for_message( sensor_dict['camera_info_topic'], CameraInfo) print('... received!') from rospy_message_converter import message_converter sensor_dict[ 'camera_info'] = message_converter.convert_ros_message_to_dictionary( camera_info_msg) # Get the kinematic chain form world_link to this sensor's parent link now = rospy.Time() print('Waiting for transformation from ' + value['link'] + ' to ' + self.world_link) self.listener.waitForTransform(value['link'], self.world_link, now, rospy.Duration(5)) print('... received!') chain = self.listener.chain(value['link'], now, self.world_link, now, self.world_link) chain_list = [] for parent, child in zip(chain[0::], chain[1::]): key = self.generateKey(parent, child) chain_list.append({ 'key': key, 'parent': parent, 'child': child }) sensor_dict['chain'] = chain_list # Add to sensor dictionary self.sensors[sensor_key] = sensor_dict sensor_labeler = InteractiveDataLabeler( self.server, self.menu_handler, sensor_dict, args['marker_size'], self.config['calibration_pattern']) self.sensor_labelers[sensor_key] = sensor_labeler print('Setup for sensor ' + sensor_key + ' is complete.') print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' + str(sensor_dict)) # print('sensor_labelers:') # print(self.sensor_labelers) self.abstract_transforms = self.getAllAbstractTransforms() # print("abstract_transforms = " + str(self.abstract_transforms)) def getTransforms(self, abstract_transforms, time=None): transforms_dict = { } # Initialize an empty dictionary that will store all the transforms for this data-stamp if time is None: time = rospy.Time.now() for ab in abstract_transforms: # Update all transformations self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0)) (trans, quat) = self.listener.lookupTransform(ab['parent'], ab['child'], time) key = self.generateKey(ab['parent'], ab['child']) transforms_dict[key] = { 'trans': trans, 'quat': quat, 'parent': ab['parent'], 'child': ab['child'] } return transforms_dict def lockAllLabelers(self): for sensor_name, sensor in self.sensors.iteritems(): self.sensor_labelers[sensor_name].lock.acquire() print("Locked all labelers") def unlockAllLabelers(self): for sensor_name, sensor in self.sensors.iteritems(): self.sensor_labelers[sensor_name].lock.release() print("Unlocked all labelers") def getLabelersTimeStatistics(self): stamps = [] # a list of the several time stamps of the stored messages for sensor_name, sensor in self.sensors.iteritems(): stamps.append( copy.deepcopy( self.sensor_labelers[sensor_name].msg.header.stamp)) max_delta = getMaxTimeDelta(stamps) # TODO : this is because of Andre's bag file problem. We should go back to the getAverageTime # average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs average_time = getMaxTime( stamps ) # For looking up transforms use average time of all sensor msgs print('Times:') for stamp, sensor_name in zip(stamps, self.sensors): printRosTime(stamp, prefix=sensor_name + ': ') return stamps, average_time, max_delta def saveCollection(self): # -------------------------------------- # Collect sensor data and labels (images, laser scans, etc) # -------------------------------------- # Lock the semaphore for all labelers self.lockAllLabelers() # Analyse message time stamps and decide if collection can be stored stamps, average_time, max_delta = self.getLabelersTimeStatistics() if max_delta is not None: # if max_delta is None (only one sensor), continue if max_delta.to_sec() > float( self.config['max_duration_between_msgs'] ): # times are close enough? rospy.logwarn('Max duration between msgs in collection is ' + str(max_delta.to_sec()) + '. Not saving collection.') self.unlockAllLabelers() return None else: # test passed rospy.loginfo('Max duration between msgs in collection is ' + str(max_delta.to_sec())) # Collect all the transforms transforms = self.getTransforms( self.abstract_transforms, average_time) # use average time of sensor msgs printRosTime(average_time, "Collected transforms for time ") all_sensor_data_dict = {} all_sensor_labels_dict = {} for sensor_key, sensor in self.sensors.iteritems(): print('collect sensor: ' + sensor_key) msg = copy.deepcopy(self.sensor_labelers[sensor_key].msg) labels = copy.deepcopy(self.sensor_labelers[sensor_key].labels) print('sensor' + sensor_key) # TODO add exception also for point cloud and depth image # Update sensor data --------------------------------------------- if sensor[ 'msg_type'] == 'Image': # Special case of requires saving image data as png separate files cv_image = self.bridge.imgmsg_to_cv2( msg, "bgr8") # Convert to opencv image and save image to disk filename = self.output_folder + '/' + sensor[ '_name'] + '_' + str(self.data_stamp) + '.jpg' filename_relative = sensor['_name'] + '_' + str( self.data_stamp) + '.jpg' cv2.imwrite(filename, cv_image) image_dict = message_converter.convert_ros_message_to_dictionary( msg) # Convert sensor data to dictionary del image_dict[ 'data'] # Remove data field (which contains the image), and replace by "data_file" image_dict[ 'data_file'] = filename_relative # Contains full path to where the image was saved # Update the data dictionary for this data stamp all_sensor_data_dict[sensor['_name']] = image_dict else: # Update the data dictionary for this data stamp all_sensor_data_dict[sensor[ '_name']] = message_converter.convert_ros_message_to_dictionary( msg) # Update sensor labels --------------------------------------------- if sensor['msg_type'] in ['Image', 'LaserScan', 'PointCloud2']: all_sensor_labels_dict[sensor_key] = labels else: raise ValueError('Unknown message type.') collection_dict = { 'data': all_sensor_data_dict, 'labels': all_sensor_labels_dict, 'transforms': transforms } self.collections[self.data_stamp] = collection_dict self.data_stamp += 1 # Save to json file D = { 'sensors': self.sensors, 'collections': self.collections, 'calibration_config': self.config } print('Saving file ' + self.output_folder + '/data_collected.json') self.createJSONFile(self.output_folder + '/data_collected.json', D) self.unlockAllLabelers() def getAllAbstractTransforms(self): # Get a list of all transforms to collect transforms_list = [] now = rospy.Time.now() all_frames = self.listener.getFrameStrings() for frame in all_frames: # print('Waiting for transformation from ' + frame + ' to ' + self.world_link + '(max 3 secs)') try: self.listener.waitForTransform(frame, self.world_link, now, rospy.Duration(3)) chain = self.listener.chain(frame, now, self.world_link, now, self.world_link) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr('Could not get transform from ' + frame + ' to ' + self.world_link + '(max 3 secs)') continue for idx in range(0, len(chain) - 1): parent = chain[idx] child = chain[idx + 1] transforms_list.append({ 'parent': parent, 'child': child, 'key': self.generateKey(parent, child) }) # https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique uniq_l = list( map(dict, frozenset(frozenset(i.items()) for i in transforms_list))) return uniq_l # get unique values def createJSONFile(self, output_file, D): print("Saving the json output file to " + str(output_file) + ", please wait, it could take a while ...") f = open(output_file, 'w') json.encoder.FLOAT_REPR = lambda f: ( "%.6f" % f) # to get only four decimal places on the json file print >> f, json.dumps(D, indent=2, sort_keys=True) f.close() print("Completed.") @staticmethod def generateKey(parent, child, suffix=''): return parent + '-' + child + suffix
class Arm(object): """Arm controls the robot's arm. Joint space control: joints = ArmJoints() # Fill out joint states arm = fetch_api.Arm() arm.move_to_joints(joints) """ def __init__(self): self._joint_client = actionlib.SimpleActionClient( JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction) self._joint_client.wait_for_server(rospy.Duration(10)) self._move_group_client = actionlib.SimpleActionClient( MOVE_GROUP_ACTION_SERVER, MoveGroupAction) self._move_group_client.wait_for_server(rospy.Duration(10)) self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._tf_listener = TransformListener() self.tuck_pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] def tuck(self): """ Uses motion-planning to tuck the arm within the footprint of the base. :return: a string describing the error, or None if there was no error """ return self.move_to_joint_goal(zip(ArmJoints.names(), self.tuck_pose)) def tuck_unsafe(self): """ TUCKS BUT DOES NOT PREVENT SELF-COLLISIONS, WHICH ARE HIGHLY LIKELY. Don't use this unless you have prior knowledge that the arm can safely return to tucked from its current configuration. Most likely, you should only use this method in simulation, where the arm can clip through the base without issue. :return: """ return self.move_to_joints(ArmJoints.from_list(self.tuck_pose)) def move_to_joints(self, joint_state): """ Moves to an ArmJoints configuration :param joint_state: an ArmJoints instance to move to :return: """ goal = control_msgs.msg.FollowJointTrajectoryGoal() goal.trajectory.joint_names.extend(ArmJoints.names()) point = trajectory_msgs.msg.JointTrajectoryPoint() point.positions.extend(joint_state.values()) point.time_from_start = rospy.Duration(TIME_FROM_START) goal.trajectory.points.append(point) self._joint_client.send_goal(goal) self._joint_client.wait_for_result(rospy.Duration(10)) def move_to_joint_goal(self, joints, allowed_planning_time=10.0, execution_timeout=15.0, group_name='arm', num_planning_attempts=1, plan_only=False, replan=False, replan_attempts=5, tolerance=0.01): """Moves the end-effector to a pose, using motion planning. Args: joints: A list of (name, value) for the arm joints. allowed_planning_time: float. The maximum duration to wait for a planning result. execution_timeout: float. The maximum duration to wait for an arm motion to execute (or for planning to fail completely), in seconds. group_name: string. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int. The number of times to compute the same plan. The shortest path is ultimately used. For random planners, this can help get shorter, less weird paths. plan_only: bool. If True, then this method does not execute the plan on the robot. Useful for determining whether this is likely to succeed. replan: bool. If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. replan_attempts: int. How many times to replan if the execution fails. tolerance: float. The goal tolerance, in meters. Returns: string describing the error if an error occurred, else None. """ goal_builder = MoveItGoalBuilder() goal_builder.set_joint_goal(*zip(*joints)) goal_builder.allowed_planning_time = allowed_planning_time goal_builder.num_planning_attempts = num_planning_attempts goal_builder.plan_only = plan_only goal_builder.planner_id = '' goal_builder.replan = replan goal_builder.replan_attempts = replan_attempts goal_builder.tolerance = tolerance goal = goal_builder.build() if goal is not None: self._move_group_client.send_goal(goal) success = self._move_group_client.wait_for_result( rospy.Duration(execution_timeout)) if not success: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) result = self._move_group_client.get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: return None else: return moveit_error_string(result.error_code.val) else: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) def move_to_pose(self, pose_stamped, allowed_planning_time=10.0, execution_timeout=15.0, group_name='arm', num_planning_attempts=1, orientation_constraint=None, plan_only=False, replan=False, replan_attempts=5, tolerance=0.01): """Moves the end-effector to a pose, using motion planning. Args: pose: geometry_msgs/PoseStamped. The goal pose for the gripper. allowed_planning_time: float. The maximum duration to wait for a planning result. execution_timeout: float. The maximum duration to wait for an arm motion to execute (or for planning to fail completely), in seconds. group_name: string. Either 'arm' or 'arm_with_torso'. num_planning_attempts: int. The number of times to compute the same plan. The shortest path is ultimately used. For random planners, this can help get shorter, less weird paths. orientation_constraint: moveit_msgs/OrientationConstraint. An orientation constraint for the entire path. plan_only: bool. If True, then this method does not execute the plan on the robot. Useful for determining whether this is likely to succeed. replan: bool. If True, then if an execution fails (while the arm is moving), then come up with a new plan and execute it. replan_attempts: int. How many times to replan if the execution fails. tolerance: float. The goal tolerance, in meters. Returns: string describing the error if an error occurred, else None. """ goal_builder = MoveItGoalBuilder() goal_builder.set_pose_goal(pose_stamped) if orientation_constraint is not None: goal_builder.add_path_orientation_contraint(orientation_constraint) goal_builder.allowed_planning_time = allowed_planning_time goal_builder.num_planning_attempts = num_planning_attempts goal_builder.plan_only = plan_only goal_builder.replan = replan goal_builder.replan_attempts = replan_attempts goal_builder.tolerance = tolerance goal = goal_builder.build() if goal is not None: self._move_group_client.send_goal(goal) self._move_group_client.wait_for_result( rospy.Duration(execution_timeout)) result = self._move_group_client.get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: return None else: return moveit_error_string(result.error_code.val) else: return moveit_error_string(MoveItErrorCodes.TIMED_OUT) def straight_move_to_pose(self, group, pose_stamped, ee_step=0.025, jump_threshold=2.0, avoid_collisions=True): """Moves the end-effector to a pose in a straight line. Args: group: moveit_commander.MoveGroupCommander. The planning group for the arm. pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper. ee_step: float. The distance in meters to interpolate the path. jump_threshold: float. The maximum allowable distance in the arm's configuration space allowed between two poses in the path. Used to prevent "jumps" in the IK solution. avoid_collisions: bool. Whether to check for obstacles or not. Returns: string describing the error if an error occurred, else None. """ # Transform pose into planning frame self._tf_listener.waitForTransform(pose_stamped.header.frame_id, group.get_planning_frame(), rospy.Time.now(), rospy.Duration(1.0)) try: pose_transformed = self._tf_listener.transformPose( group.get_planning_frame(), pose_stamped) except (tf.LookupException, tf.ConnectivityException): rospy.logerr('Unable to transform pose from frame {} to {}'.format( pose_stamped.header.frame_id, group.get_planning_frame())) return moveit_error_string( MoveItErrorCodes.FRAME_TRANSFORM_FAILURE) # Compute path plan, fraction = group.compute_cartesian_path( [group.get_current_pose().pose, pose_transformed.pose], ee_step, jump_threshold, avoid_collisions) if fraction < 1 and fraction > 0: rospy.logerr('Only able to compute {}% of the path'.format( fraction * 100)) if fraction == 0: return moveit_error_string(MoveItErrorCodes.PLANNING_FAILED) # Execute path result = group.execute(plan, wait=True) if not result: return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN) else: return None def check_pose(self, pose_stamped, allowed_planning_time=10.0, group_name='arm', tolerance=0.01): return self.move_to_pose(pose_stamped, allowed_planning_time=allowed_planning_time, group_name=group_name, tolerance=tolerance, plan_only=True) def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): """Computes inverse kinematics for the given pose. Note: if you are interested in returning the IK solutions, we have shown how to access them. Args: pose_stamped: geometry_msgs/PoseStamped. timeout: rospy.Duration. How long to wait before giving up on the IK solution. Returns: A list of (name, value) for the arm joints if the IK solution was found, False otherwise. """ request = GetPositionIKRequest() request.ik_request.pose_stamped = pose_stamped request.ik_request.group_name = 'arm' request.ik_request.timeout = timeout response = self._compute_ik(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False joint_state = response.solution.joint_state joints = [] for name, position in zip(joint_state.name, joint_state.position): if name in ArmJoints.names(): joints.append((name, position)) return joints def cancel_all_goals(self): self._move_group_client.cancel_all_goals() self._joint_client.cancel_all_goals()