def __init__(self, parent_instance_handle, server_name, ik_solver): self.parent_instance_handle = parent_instance_handle self.server = InteractiveMarkerServer(server_name) self.ik_solver = ik_solver self.ik_position_xyz_bounds = [0.01, 0.01, 0.01] self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK self._menu_handlers = {} self._menu_cmds = {} self.menu_handler = MenuHandler() del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) self._int_marker_name_list = [] self.markers_created_cnt = 0 self.marker_cnt = 0 self.name_to_marker_dict = {} self.waypoint_to_marker_dict = {} self.name_to_waypoint_dict = {}
def __init__(self, name, pose, dimensions, db_id): """ Constructs a WorldLandmark object. Use bounding_box() or cloud_box() instead. Args: name (str): Name of this landmark pose (Pose): Pose of the landmark, in base frame dimensions (Vector3): Size of bounding box db_id (str): The MongoDB id if this is a cloud box """ self._name = name self._pose = pose self._dimensions = dimensions self._db_id = db_id if self.is_bounding_box(): self.object = Landmark(type=Landmark.TABLE_TOP, name=name, pose=pose, dimensions=dimensions, db_id='') elif self.is_cloud_box(): self.object = Landmark(type=Landmark.CLOUD_BOX, name=name, pose=pose, dimensions=dimensions, db_id=db_id) self.int_marker = None self.is_removed = False # TODO(jstn): Move this outside. self.menu_handler = MenuHandler() self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() # self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1) # self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1) self.pub = rospy.Publisher('/pose_to_follow', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) #self.global_frame_id = 'world' self.global_frame_id = 'base_footprint' pose = Pose() pose.position.x = 0.2 # pose.position.y = 0.35 pose.position.y = -0.35 #pose.position.z = 0.1 pose.position.z = 1.1 pose.orientation.w = 1.0 # pose.orientation.x = 0.35762 # pose.orientation.y = 0.50398 # pose.orientation.z = 0.45213 # pose.orientation.w = 0.64319 #self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges()
def __init__(self, root_tips, suffix=u''): """ :param root_tips: list containing root->tip tuple for each interactive marker. :type root_tips: list :param suffix: the marker will be called 'eef_control{}'.format(suffix) :type suffix: str """ self.enable_self_collision = rospy.get_param(u'~enable_self_collision', True) self.giskard = GiskardWrapper() if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.markers = {} # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, root, tip, self.giskard, self.enable_self_collision)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, root_link, tip_links): # tf self.tfBuffer = Buffer(rospy.Duration(1)) self.tf_listener = TransformListener(self.tfBuffer) # giskard goal client # self.client = SimpleActionClient('move', ControllerListAction) self.client = SimpleActionClient('qp_controller/command', ControllerListAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer("eef_control") self.menu_handler = MenuHandler() for tip_link in tip_links: int_marker = self.make6DofMarker( InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link) self.server.insert( int_marker, self.process_feedback(self.server, self.client, root_link, tip_link)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1): int_marker = InteractiveMarker() int_marker.header.frame_id = path_msg.header.frame_id int_marker.name = str(path_id) int_marker.description = "Path {0}".format(path_id) # line_marker = Marker() # line_marker.type = Marker.LINE_STRIP # line_marker.scale.x = width # line_marker.color = color_msg # line_marker.points = [p.pose.position for p in path_msg.poses] # for point in line_marker.points: # point.z += delta_z control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MENU # control.markers.append(line_marker) points = [node(pose, delta_z) for pose in path_msg.poses] for p1, p2 in zip(points[:-1], points[1:]): control.markers.append(cylinder_between(p1, p2, color_msg, width)) for p in points: control.markers.append(sphere_at(p, color_msg, width)) int_marker.controls.append(copy.deepcopy(control)) menu_handler = MenuHandler() # put all the information in the main menu #d = menu_handler.insert("Description") for line in description: menu_handler.insert(line)#, parent=d) return menu_handler, int_marker
def _update_menu(self): '''Recreates the menu when something has changed''' self._menu_handler = MenuHandler() frame_entry = self._menu_handler.insert('Reference frame') self._sub_entries = [None] * len(ActionStepMarker._ref_names) for i in range(len(ActionStepMarker._ref_names)): self._sub_entries[i] = self._menu_handler.insert( ActionStepMarker._ref_names[i], parent=frame_entry, callback=self.change_ref_cb) self._menu_handler.insert('Move arm here', callback=self.move_to_cb) self._menu_handler.insert('Move to current arm pose', callback=self.move_pose_to_cb) self._menu_handler.insert('Delete', callback=self.delete_step_cb) for i in range(len(ActionStepMarker._ref_names)): self._menu_handler.setCheckState(self._sub_entries[i], MenuHandler.UNCHECKED) menu_id = self._get_menu_id(self._get_ref_name()) if menu_id == None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
def init_menu(self): self.menu_handler = MenuHandler() # Remove Goal entry self.menu_remove_goal_title = "Remove Goal" self.menu_remove_goal_handle = self.menu_handler.insert( self.menu_remove_goal_title, callback=self.process_feedback)
def __init__(self, server): self.server = server self.pose = Pose() self.publish = True self.make_marker() self.menu_handler = MenuHandler() item = self.menu_handler.insert("publish", callback=self.menu_callback) self.menu_handler.setCheckState(item, MenuHandler.CHECKED) self.menu_handler.apply(self.server, self.marker_name)
def __init__(self, server, id_, name, initial_pose=None): self._menu_handler = MenuHandler() self._menu_handler.insert("Delete", callback=self.deleteCallback) self._server = server self._key = name + str(id_) self._marker_map = {} self._callback_map = {} # not all templates will have a dynamic reconfigure server self._dserver = None self._initial_pose = initial_pose
def __init__(self, pose, index, dimensions, is_recognized): ''' Initialization of objects''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server()
def __init__(self, server): self.server = server self.pose = Pose() self.active = True self.covariance = 0.1 self.int_marker = None self.make_marker() self.menu_handler = MenuHandler() item = self.menu_handler.insert("active", callback=self.menu_callback) self.menu_handler.setCheckState(item, MenuHandler.CHECKED) high_conf = self.menu_handler.insert("high confidence", callback=self.conf_callback) self.menu_handler.setCheckState(high_conf, MenuHandler.CHECKED) self.menu_handler.apply(self.server, self.marker_name)
def _update_menu(self): """Recreates the menu when something has changed.""" self._menu_handler = MenuHandler() # Insert sub entries. self._sub_entries = [] self._menu_options = [] frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref']) refs = ActionStepMarker._ref_names for ref in refs: if ref != BASE_LINK and "Obj" not in str(ref): subent = self._menu_handler.insert(ref + ' center', parent=frame_entry, callback=self.subent_cb) self._sub_entries += [subent] self._menu_options += [subent] elif ref == BASE_LINK: subent = self._menu_handler.insert(ref, parent=frame_entry, callback=self.subent_cb) self._sub_entries += [subent] self._menu_options += [subent] select_one = self._menu_handler.insert('Select an object side', parent=frame_entry, callback=self.select_one_cb) self._menu_options += [select_one] # Inset main menu entries. self._menu_handler.insert(MENU_OPTIONS['move_here'], callback=self.move_to_cb) self._menu_handler.insert(MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb) self._menu_handler.insert(MENU_OPTIONS['del'], callback=self.delete_step_cb) # Make all unchecked to start. for subent in self._sub_entries: self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED) self._menu_handler.setCheckState(select_one, MenuHandler.UNCHECKED) # Check if necessary. Set base_link to checked menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) # Update. self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
class WorldLandmark: '''Class for representing objects''' def __init__(self, remove_object_cb, pose, index, dimensions, is_recognized): ''' Args: remove_object_cb (callback) : execute when removing objects pose (Pose): Position of bounding box index (int): For naming object in world (e.g. "thing 0") dimensions (Vector3): Size of bounding box is_recognized (bool): Result of object recognition. ''' self._remove_object_cb = remove_object_cb self._assigned_name = None self.index = index self.is_recognized = is_recognized self.object = Landmark(self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) # ################################################################## # Instance methods: Public (API) # ################################################################## def get_name(self): '''Return this object's name. Returns: str ''' if self._assigned_name is None: if self.is_recognized: return 'object ' + str(self.index) else: return 'thing ' + str(self.index) else: return self._assigned_name def remove(self, feedback): '''Function for removing object from the world. Args: feedback (InteractiveMarkerFeedback): Unused ''' rospy.loginfo('Will remove object: ' + self.get_name()) self._remove_object_cb(self.object.name)
def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient( r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient( l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik
def init(self): # The parameter /robot_description must exist if not rospy.has_param('/robot_description'): rospy.logerr( "Parameter '/robot_description' must exist to continue") sys.exit(1) self.config = loadJSONConfig(self.args['calibration_file']) if self.config is None: sys.exit(1) # loadJSON should tell you why. # self.config = CalibConfig() # ok = self.config.loadJSON(self.args['calibration_file']) # # Exit if it fails to open a valid calibration file # if not ok: # sys.exit(1) # Load the urdf from /robot_description self.urdf = URDF.from_parameter_server() # ok = validateLinks(self.config.world_link, self.config.sensors, self.urdf) # if not ok: # sys.exit(1) print('Number of sensors: ' + str(len(self.config['sensors']))) # Init interaction self.server = InteractiveMarkerServer("interactive_first_guess") self.menu = MenuHandler() self.menu.insert("Save sensors configuration", callback=self.onSaveFirstGuess) self.menu.insert("Reset to initial configuration", callback=self.onReset) # For each node generate an interactive marker. for name, sensor in self.config['sensors'].items(): print(Fore.BLUE + '\nSensor name is ' + name + Style.RESET_ALL) params = { "frame_world": self.config['world_link'], "frame_opt_parent": sensor['parent_link'], "frame_opt_child": sensor['child_link'], "frame_sensor": sensor['link'], "marker_scale": self.args['marker_scale'] } # Append to the list of sensors self.sensors.append(Sensor(name, self.server, self.menu, **params)) self.server.applyChanges()
def __init__(self, side_prefix): if GripperMarkers._tf_listener is None: GripperMarkers._tf_listener = TransformListener() self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers') self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def __init__(self, pose, index, dimensions, is_recognized): ''' Args: pose (Pose): Position of bounding box index (int): For naming object in world (e.g. "thing 0") dimensions (Vector3): Size of bounding box is_recognized (bool): Result of object recognition. ''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Landmark( Landmark.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove)
def update(self): '''Updates marker/arm loop''' self._menu_handler = MenuHandler() # Inset main menu entries. self._menu_handler.insert('Move gripper here', callback=self._move_to_cb) self._menu_handler.insert('Move marker to current gripper pose', callback=self._move_pose_to_cb) if self._is_hand_open(): self._menu_handler.insert('Close gripper', callback=self._close_gripper_cb) else: self._menu_handler.insert('Open gripper', callback=self._open_gripper_cb) frame_id = REF_FRAME pose = self.get_pose() # if self._marker_moved() or self._menu_control is None: rospy.loginfo("Marker moved") menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) self._menu_control = menu_control # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose.pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(self._menu_control) ArmControlMarker._im_server.insert(int_marker, self._marker_feedback_cb) self._menu_handler.apply(ArmControlMarker._im_server, self._get_name()) ArmControlMarker._im_server.applyChanges()
class WorldObject: '''Class for representing objects''' default_color = ColorRGBA(0.2, 0.8, 0.0, 0.6) fake_color = ColorRGBA(0.4, 0.6, 0.4, 0.6) selected_colors = [ColorRGBA(0.6, 0.6, 0.0, 0.6), ColorRGBA(0.6, 0.6, 0.2, 0.6)] def __init__(self, pose, index, dimensions, is_recognized, is_fake=False): ''' Initialization of objects''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) self.is_marker = False self.is_fake = is_fake def remove(self, dummy): '''Function for removing object from the world''' rospy.loginfo('Will remove object' + self.get_name()) self.is_removed = True def assign_name(self, name): '''Function for assigning a different name''' self.assigned_name = name self.object.name = name def get_name(self): '''Function to get the object name''' if (self.assigned_name is None): if (self.is_recognized): return 'object' + str(self.index) else: return 'thing' + str(self.index) else: return self.assigned_name def decrease_index(self): '''Function to decrese object index''' self.index -= 1
class WorldLandmark: '''Class for representing objects''' def __init__(self, pose, index, dimensions, is_recognized): ''' Args: pose (Pose): Position of bounding box index (int): For naming object in world (e.g. "thing 0") dimensions (Vector3): Size of bounding box is_recognized (bool): Result of object recognition. ''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Landmark( Landmark.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) def get_name(self): '''Return this object's name. Returns: str ''' if self.assigned_name is None: if self.is_recognized: return 'object ' + str(self.index) else: return 'thing ' + str(self.index) else: return self.assigned_name def remove(self, __): '''Function for removing object from the world. Args: __ (???): Unused ''' rospy.loginfo('Will remove object: ' + self.get_name()) self.is_removed = True
def __init__(self, pose, index, dimensions, is_recognized): """ Initialization of objects""" self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert("Remove from scene", callback=self.remove)
def __init__(self, pose, index, dimensions, marker_id=None): ''' Initialization of objects''' self.index = index self.assigned_name = None self.position = pose.position self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) self.marker_id = marker_id
def start_once(self): # giskard goal client self.client = SimpleActionClient(u'qp_controller/command', MoveAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() all_goals = {} for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, self.client, root, tip, all_goals)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik
def __init__(self, server, name, X_WM, markers): self._server = server self.name = name self._X_WM = X_WM self._menu = MenuHandler() self._edit_id = self._menu.insert("Edit", callback=self._edit_callback) self._delete_id = self._menu.insert("Delete", callback=self._delete_callback) self._menu.insert(self.name, callback=noop) # For annotation. self._t_click_prev = -float('inf') # Make template marker. template = InteractiveMarker() template.name = self.name template.description = name template.header.frame_id = "world" viz = InteractiveMarkerControl() viz.interaction_mode = InteractiveMarkerControl.BUTTON viz.always_visible = True viz.markers.extend(markers) template.controls.append(viz) self._template = template # Initialize. self._edit = False self._update()
class WorldObject: """Class for representing objects""" def __init__(self, pose, index, dimensions, is_recognized): """ Initialization of objects""" self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert("Remove from scene", callback=self.remove) def remove(self, dummy): """Function for removing object from the world""" rospy.loginfo("Will remove object" + self.get_name()) self.is_removed = True def assign_name(self, name): """Function for assigning a different name""" self.assigned_name = name self.object.name = name def get_name(self): """Function to get the object name""" if self.assigned_name == None: if self.is_recognized: return "object" + str(self.index) else: return "thing" + str(self.index) else: return self.assigned_name def decrease_index(self): """Function to decrese object index""" self.index -= 1
def _update_menu(self): '''Recreates the menu when something has changed.''' self._menu_handler = MenuHandler() # Insert sub entries. self._sub_entries = [] frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref']) refs = ActionStepMarker._ref_names for ref in refs: subent = self._menu_handler.insert(ref, parent=frame_entry, callback=self.change_ref_cb) self._sub_entries += [subent] # Inset main menu entries. self._menu_handler.insert(MENU_OPTIONS['move_here'], callback=self.move_to_cb) self._menu_handler.insert(MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb) self._menu_handler.insert(MENU_OPTIONS['del'], callback=self.delete_step_cb) # Make all unchecked to start. for subent in self._sub_entries: self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED) # Check if necessary. menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) # Update. self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
class WorldObject: '''Class for representing objects''' def __init__(self, pose, index, dimensions, is_recognized): ''' Initialization of objects''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) def remove(self, dummy): '''Function for removing object from the world''' rospy.loginfo('Will remove object' + self.get_name()) self.is_removed = True def assign_name(self, name): '''Function for assigning a different name''' self.assigned_name = name self.object.name = name def get_name(self): '''Function to get the object name''' if (self.assigned_name == None): if (self.is_recognized): return 'object' + str(self.index) else: return 'thing' + str(self.index) else: return self.assigned_name def decrease_index(self): '''Function to decrese object index''' self.index -= 1
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server()
class WorldObject: '''Class for representing objects''' def __init__(self, pose, index, dimensions, marker_id=None): ''' Initialization of objects''' self.index = index self.assigned_name = None self.position = pose.position self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove) self.marker_id = marker_id def remove(self, dummy): '''Function for removing object from the world''' rospy.loginfo('Will remove object' + self.get_name()) self.is_removed = True def assign_name(self, name): '''Function for assigning a different name''' self.assigned_name = name self.object.name = name def get_name(self): '''Function to get the object name''' if (self.assigned_name == None): return 'marker' + str(self.marker_id) else: return self.assigned_name def decrease_index(self): '''Function to decrese object index''' self.index -= 1
def __init__(self, server, cam_info): self.cam_info = cam_info self.marker_name = "goal" self.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE self.absolute_publisher = rospy.Publisher("goal_absolute", PoseWithCertaintyArray, queue_size=1) self.relative_publisher = rospy.Publisher("goal_relative", PoseWithCertaintyArray, queue_size=1) self.relative_posts_publisher = rospy.Publisher("goal_posts_relative", PoseWithCertaintyArray, queue_size=1) super(GoalMarker, self).__init__(server) self.menu_handler = MenuHandler() self.pose.position.x = 3.0
def __init__(self, side_prefix): if GripperMarkers._tf_listener is None: GripperMarkers._tf_listener = TransformListener() self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers') self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
def __init__(self, side_prefix): self._ik_service = IK(side_prefix) r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix)) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def __init__(self, topic, initial_x, initial_y, initial_z, initial_roll, initial_pitch, initial_yaw, frame_id): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.resolved_name)) pose = Pose() pose.position.x = initial_x pose.position.y = initial_y pose.position.z = initial_z quat = quaternion_from_euler(initial_roll, initial_pitch, initial_yaw) pose.orientation = Quaternion(*quat) self.initial_pose = pose self.frame_id = frame_id # self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
class ActionStepMarker: ''' Marker for visualizing the steps of an action''' _im_server = None _offset = 0.09 _ref_object_list = None _ref_names = None _marker_click_cb = None def __init__(self, step_number, arm_index, action_step, marker_click_cb): if ActionStepMarker._im_server == None: im_server = InteractiveMarkerServer('programmed_actions') ActionStepMarker._im_server = im_server self.action_step = action_step self.arm_index = arm_index self.step_number = step_number self.is_requested = False self.is_deleted = False self.is_control_visible = False self.is_edited = False self.has_object = False self._sub_entries = None self._menu_handler = None ActionStepMarker._marker_click_cb = marker_click_cb def _is_reachable(self): '''Checks if there is an IK solution for action step''' dummy, is_reachable = Arms.solve_ik_for_arm(self.arm_index, self.get_target()) rospy.loginfo('Reachability of pose in ActionStepMarker : ' + str(is_reachable)) return is_reachable def get_uid(self): '''Returns a unique id of the marker''' return (2 * self.step_number + self.arm_index) def _get_name(self): '''Generates the unique name for the marker''' return ('step' + str(self.step_number) + 'arm' + str(self.arm_index)) def decrease_id(self): '''Reduces the index of the marker''' self.step_number -= 1 self._update_menu() def update_ref_frames(self, ref_frame_list): '''Updates and re-assigns coordinate frames when the world changes''' # There is a new list of objects # If the current frames are already assigned to object, # we need to figure out the correspondences ActionStepMarker._ref_object_list = ref_frame_list arm_pose = self.get_target() if (arm_pose.refFrame == ArmState.OBJECT): prev_ref_obj = arm_pose.refFrameObject new_ref_obj = World.get_most_similar_obj(prev_ref_obj, ref_frame_list) self.has_object = False if (new_ref_obj != None): self.has_object = True arm_pose.refFrameObject = new_ref_obj ActionStepMarker._ref_names = ['base_link'] for i in range(len(ActionStepMarker._ref_object_list)): ActionStepMarker._ref_names.append( ActionStepMarker._ref_object_list[i].name) self._update_menu() def destroy(self): '''Removes marker from the world''' ActionStepMarker._im_server.erase(self._get_name()) ActionStepMarker._im_server.applyChanges() def _update_menu(self): '''Recreates the menu when something has changed''' self._menu_handler = MenuHandler() frame_entry = self._menu_handler.insert('Reference frame') self._sub_entries = [None] * len(ActionStepMarker._ref_names) for i in range(len(ActionStepMarker._ref_names)): self._sub_entries[i] = self._menu_handler.insert( ActionStepMarker._ref_names[i], parent=frame_entry, callback=self.change_ref_cb) self._menu_handler.insert('Move arm here', callback=self.move_to_cb) self._menu_handler.insert('Move to current arm pose', callback=self.move_pose_to_cb) self._menu_handler.insert('Delete', callback=self.delete_step_cb) for i in range(len(ActionStepMarker._ref_names)): self._menu_handler.setCheckState(self._sub_entries[i], MenuHandler.UNCHECKED) menu_id = self._get_menu_id(self._get_ref_name()) if menu_id == None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges() def _get_menu_id(self, ref_name): '''Returns the unique menu id from its name None if the object is not found''' if ref_name in ActionStepMarker._ref_names: index = ActionStepMarker._ref_names.index(ref_name) return self._sub_entries[index] else: return None def _get_menu_name(self, menu_id): '''Returns the menu name from its unique menu id''' index = self._sub_entries.index(menu_id) return ActionStepMarker._ref_names[index] def _get_ref_name(self): '''Returns the name string for the reference frame object of the action step''' ref_name = None if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: ref_frame = self.action_step.armTarget.rArm.refFrame ref_name = self.action_step.armTarget.rArm.refFrameObject.name else: ref_frame = self.action_step.armTarget.lArm.refFrame ref_name = self.action_step.armTarget.lArm.refFrameObject.name elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): if self.arm_index == 0: ref_frame = self.action_step.armTrajectory.rRefFrame ref_name = self.action_step.armTrajectory.rRefFrameObject.name else: ref_frame = self.action_step.armTrajectory.lRefFrame ref_name = self.action_step.armTrajectory.lRefFrameObject.name else: rospy.logerr('Unhandled marker type: ' + str(self.action_step.type)) if (ref_frame == ArmState.ROBOT_BASE): ref_name = 'base_link' return ref_name def _set_ref(self, new_ref_name): '''Changes the reference frame of the action step''' new_ref = World.get_ref_from_name(new_ref_name) if (new_ref != ArmState.ROBOT_BASE): index = ActionStepMarker._ref_names.index(new_ref_name) new_ref_obj = ActionStepMarker._ref_object_list[index - 1] else: new_ref_obj = Object() if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: self.action_step.armTarget.rArm = World.convert_ref_frame( self.action_step.armTarget.rArm, new_ref, new_ref_obj) else: self.action_step.armTarget.lArm = World.convert_ref_frame( self.action_step.armTarget.lArm, new_ref, new_ref_obj) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): for i in range(len(self.action_step.armTrajectory.timing)): if self.arm_index == 0: arm_old = self.action_step.armTrajectory.rArm[i] arm_new = World.convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.rArm[i] = arm_new else: arm_old = self.action_step.armTrajectory.lArm[i] arm_new = World.convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.lArm[i] = arm_new if self.arm_index == 0: self.action_step.armTrajectory.rRefFrameObject = new_ref_obj self.action_step.armTrajectory.rRefFrame = new_ref else: self.action_step.armTrajectory.lRefFrameObject = new_ref_obj self.action_step.armTrajectory.lRefFrame = new_ref def _is_hand_open(self): '''Checks if the gripper is open for the action step''' if self.arm_index == 0: gripper_state = self.action_step.gripperAction.rGripper else: gripper_state = self.action_step.gripperAction.lGripper return (gripper_state == GripperState.OPEN) def set_new_pose(self, new_pose): '''Changes the pose of the action step''' if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: pose = ActionStepMarker._offset_pose(new_pose, -1) self.action_step.armTarget.rArm.ee_pose = pose else: pose = ActionStepMarker._offset_pose(new_pose, -1) self.action_step.armTarget.lArm.ee_pose = pose self.update_viz() elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): rospy.logwarn('Modification of whole trajectory ' + 'segments is not implemented.') def get_absolute_position(self, is_start=True): '''Returns the absolute position of the action step''' pose = self.get_absolute_pose(is_start) return pose.position def get_absolute_pose(self, is_start=True): '''Returns the absolute pose of the action step''' if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: arm_pose = self.action_step.armTarget.rArm else: arm_pose = self.action_step.armTarget.lArm elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): if self.arm_index == 0: if is_start: index = len(self.action_step.armTrajectory.rArm) - 1 arm_pose = self.action_step.armTrajectory.rArm[index] else: arm_pose = self.action_step.armTrajectory.rArm[0] else: if is_start: index = len(self.action_step.armTrajectory.lArm) - 1 arm_pose = self.action_step.armTrajectory.lArm[index] else: arm_pose = self.action_step.armTrajectory.lArm[0] #if (arm_pose.refFrame == ArmState.OBJECT and # World.has_object(arm_pose.refFrameObject.name)): # return ActionStepMarker._offset_pose(arm_pose.ee_pose) #else: world_pose = World.get_absolute_pose(arm_pose) return ActionStepMarker._offset_pose(world_pose) def get_pose(self): '''Returns the pose of the action step''' target = self.get_target() if (target != None): return ActionStepMarker._offset_pose(target.ee_pose) @staticmethod def _offset_pose(pose, constant=1): '''Offsets the world pose for visualization''' transform = World.get_matrix_from_pose(pose) offset_array = [constant * ActionStepMarker._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return World.get_pose_from_transform(hand_transform) def set_target(self, target): '''Sets the new pose for the action step''' if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: self.action_step.armTarget.rArm = target else: self.action_step.armTarget.lArm = target self.has_object = True self._update_menu() self.is_edited = False def get_target(self, traj_index=None): '''Returns the pose for the action step''' if (self.action_step.type == ActionStep.ARM_TARGET): if self.arm_index == 0: return self.action_step.armTarget.rArm else: return self.action_step.armTarget.lArm elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): if self.arm_index == 0: if traj_index == None: traj = self.action_step.armTrajectory.rArm traj_index = int(len(traj) / 2) return self.action_step.armTrajectory.rArm[traj_index] else: if traj_index == None: traj = self.action_step.armTrajectory.lArm traj_index = int(len(traj) / 2) return self.action_step.armTrajectory.lArm[traj_index] def _get_traj_pose(self, index): '''Returns a single pose for trajectory''' if (self.action_step.type == ActionStep.ARM_TRAJECTORY): if self.arm_index == 0: target = self.action_step.armTrajectory.rArm[index] else: target = self.action_step.armTrajectory.lArm[index] return target.ee_pose else: rospy.logerr('Cannot request trajectory pose ' + 'on non-trajectory action step.') def _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ActionStep.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.03, 0.04), header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.8, 0.2, 0.5), points=[pose.position, Point(0, 0, 0)])) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0, 0, 0.03), text='Step' + str(self.step_number), color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb) @staticmethod def make_sphere_marker(uid, pose, frame_id, radius): '''Creates a sphere marker''' return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2), scale=Vector3(radius, radius, radius), pose=pose, header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.5, 0.0, 0.8)) def marker_feedback_cb(self, feedback): '''Callback for when an event occurs on the marker''' if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.set_new_pose(feedback.pose) self.update_viz() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: # Set the visibility of the 6DOF controller rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True ActionStepMarker._marker_click_cb(self.get_uid(), self.is_control_visible) else: rospy.loginfo('Unknown event' + str(feedback.event_type)) def delete_step_cb(self, dummy): '''Callback for when delete is requested''' self.is_deleted = True def move_to_cb(self, dummy): '''Callback for when moving to a pose is requested''' self.is_requested = True def move_pose_to_cb(self, dummy): '''Callback for when a pose change to current is requested''' self.is_edited = True def pose_reached(self): '''Update when a requested pose is reached''' self.is_requested = False def change_ref_cb(self, feedback): '''Callback for when a reference frame change is requested''' self._menu_handler.setCheckState( self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED) self._menu_handler.setCheckState(feedback.menu_entry_id, MenuHandler.CHECKED) new_ref = self._get_menu_name(feedback.menu_entry_id) self._set_ref(new_ref) rospy.loginfo('Switching reference frame to ' + new_ref + ' for action step ' + self._get_name()) self._menu_handler.reApply(ActionStepMarker._im_server) ActionStepMarker._im_server.applyChanges() self.update_viz() def update_viz(self): '''Updates visualization fully''' self._update_viz_core() self._menu_handler.reApply(ActionStepMarker._im_server) ActionStepMarker._im_server.applyChanges() def _add_6dof_marker(self, int_marker, is_fixed): '''Adds a 6 DoF control marker to the interactive marker''' control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): '''Creates one component of the 6dof controller''' control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 if self._is_reachable(): mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6) return mesh def _make_gripper_marker(self, control, is_hand_open=False): '''Makes a gripper marker''' if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -ActionStepMarker._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger.dae') mesh2.pose = World.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger_tip.dae') mesh3.pose = World.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger.dae') mesh4.pose = World.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger_tip.dae') mesh5.pose = World.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control
class MarkerTemplate(object): def __init__(self, server, id_, name, initial_pose=None): self._menu_handler = MenuHandler() self._menu_handler.insert("Delete", callback=self.deleteCallback) self._server = server self._key = name + str(id_) self._marker_map = {} self._callback_map = {} # not all templates will have a dynamic reconfigure server self._dserver = None self._initial_pose = initial_pose @property def initial_pose(self): """Return a marker's initial pose if it was passed in.""" return self._initial_pose # @property # def key(self): # """Get the marker template's key on the server.""" # key = self._key + str(self._id) # return key @property def server(self): """Get the marker template's interactive marker server.""" return self._server @property def marker_map(self): """Get the interactive marker map of this marker template.""" return self._marker_map @property def callback_map(self): """Get the callback map of this marker template.""" return self._callback_map @property def menu_handler(self): """Get the menu handler of this marker template.""" return self._menu_handler @property def setNudgeDistance(self, distance): self._nudgeDistance = distance def getNudgeDistance(self): return self._nudgeDistance def initNudgeMenu(self): nudgeControl = self.menu_handler.insert("Nudge hand " + str(self._nudgeDistance) + "m") self.menu_handler.insert("World X", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeX) self.menu_handler.insert("World Y", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeY) self.menu_handler.insert("World Z", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeZ) self.menu_handler.insert("World -X", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeXNeg) self.menu_handler.insert("World -Y", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeYNeg) self.menu_handler.insert("World -Z", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeZNeg) def addInteractiveMarker(self, marker, callback=None): name = marker.name self._marker_map[name] = marker if callback: self._callback_map[name] = callback return self.server.insert(marker, callback) else: self._callback_map[name] = self.processFeedback return self.server.insert(marker, self.processFeedback) def removeInteractiveMarker(self, marker_name): if self.server.erase(marker_name): if marker_name in self._marker_map: del self._marker_map[marker_name] del self._callback_map[marker_name] return True return False def attachMenuHandler(self, marker): return self.menu_handler.apply(self.server, marker.name) def getMarker(self): return self.server.get(self._key) def hasMarker(self): if self._key in self._marker_map.keys(): return True else: return False def deleteCallback(self, feedback): for key in self._marker_map.keys(): self.server.erase(key) self.server.applyChanges() self.tearDown() def tearDown(self, keepAlive=False): # forcefully shut down service before killing node if self._dserver: self._dserver.set_service.shutdown("User deleted template.") # for rostest (and potentially other cases), we want to clean up but keep the node alive if not keepAlive: rospy.signal_shutdown("User deleted template.") def processFeedback(self, feedback): self.server.applyChanges()
def create_menu(self): """ Create and populates all the menu entries """ menu_handler = MenuHandler() menu_handler.insert("Point the head", callback = self.move_head) menu_handler.insert("Add position to trajectory", callback = self.add_point) menu_handler.insert("Place marker over gripper", callback = self.place_gripper) menu_handler.insert("Execute trjectory", callback = self.execute_trajectory) menu_handler.insert("Clear trajectory", callback = self.clear_trajectory) menu_handler.insert("Publish trajectory", callback = self.publish_trajectory) menu_handler.insert("Move the arm (planning)", callback = self.plan_arm) menu_handler.insert("Move the arm (collision-free)", callback = self.collision_free_arm) menu_handler.insert("Move the arm to trajectory start (collision-free)", callback = self.arm_trajectory_start) menu_handler.insert("Update planning scene", callback = self.update_planning) menu_handler.apply(self.server, self.int_marker.name)
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server() def get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except: rospy.logerr('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): rospy.loginfo('You pressed the `Move arm here` button from the menu.') joint_positions = self.ik.get_ik_for_ee(self.ee_pose) print 'joint: ' + str(joint_positions) if (joint_positions != None): self.toggle_arm(self.side_prefix, 'Freeze', False) #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309] print 'returned from IK: ' + str(joint_positions) #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]) #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124] self.move_to_joints(self.side_prefix, list(joint_positions), 5.0) print 'done' def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' print 'toggle' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str( pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open = False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self._ik_service = IK(side_prefix) r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix)) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() def get_ee_pose(self): from_frame = '/base_link' to_frame = '/' + self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except Exception as e: rospy.logerr('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] import traceback traceback.print_exc() return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) self.move_to_joints(self.side_prefix, [ik_sol], 1.0) def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def marker_cb(self, pose_markers): rospy.loginfo('AR Marker Pose updating') transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose) offset_array = [0, 0, .03] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform) self.update_viz() def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = "x=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) label_pos = Point() label_pos.x = pose.position.x label_pos.y = pose.position.y label_pos.z = pose.position.z label = "{} arm".format(self.side_prefix) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=label, color=ColorRGBA(0.0, 0.0, 0.0, 0.9), header=Header(frame_id=frame_id), pose=Pose(label_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1,transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def _offset_pose(pose): transform = GripperMarkers.get_matrix_from_pose(pose) offset_array = [-GripperMarkers._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return GripperMarkers.get_pose_from_transform(hand_transform) @staticmethod def get_matrix_from_pose(pose): rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) if ik_sol == None: mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) time_move = time_to_joint print "using following positions %s" % positions for pose in positions: velocities = [0] * len(pose) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose, velocities=velocities, time_from_start=rospy.Duration(time_move))) time_move += time_to_joint if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal)
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() def get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except: rospy.logerr('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): rospy.loginfo('You pressed the `Move arm here` button from the menu.') joint_positions = self.ik.get_ik_for_ee(self.ee_pose) print 'joint: ' + str(joint_positions) if(joint_positions != None): self.toggle_arm(self.side_prefix, 'Freeze', False) #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309] print 'returned from IK: ' + str(joint_positions) #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]) #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124] self.move_to_joints(self.side_prefix, list(joint_positions), 5.0) print 'done' def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' print 'toggle' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1,transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik def get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame): t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) # t = rospy.Time.now() (pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :( else: rospy.logerr('TF frames do not exist; could not get end effector pose.') except Exception as err: rospy.logerr('Could not get end effector pose through TF.') rospy.logerr(err) pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, feedback): rospy.loginfo('You pressed the `Move arm here` button from the menu.') '''Moves the arm to the desired joints''' print feedback time_to_joint = 2.0 positions = self.ik.get_ik_for_ee(feedback.pose) velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) pass def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker_' + self.side_prefix int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1,transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 print self ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution is None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
class ActionStepMarker: '''Marker for visualizing the steps of an action.''' _im_server = None _offset = DEFAULT_OFFSET _ref_object_list = None _ref_names = None _marker_click_cb = None def __init__(self, step_number, arm_index, action_step, marker_click_cb): ''' Args: step_number (int): The 1-based index of the step. arm_index (int): Side.RIGHT or Side.LEFT action_step (ActionStep): The action step this marker marks. marker_click_cb (function(int,bool)): The function to call when a marker is clicked. Pass the uid of the marker (as calculated by get_uid(...) as well as whether it's selected. ''' if ActionStepMarker._im_server is None: im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) ActionStepMarker._im_server = im_server self.action_step = action_step self.arm_index = arm_index self.arm_name = ARM_NAMES[arm_index] self.step_number = step_number self.is_requested = False self.is_deleted = False self.is_control_visible = False self.is_edited = False self.has_object = False self._sub_entries = None self._menu_handler = None self._prev_is_reachable = None ActionStepMarker._marker_click_cb = marker_click_cb # ################################################################## # Static methods: Public (API) # ################################################################## @staticmethod def calc_uid(arm_index, step_number): '''Returns a unique id of the marker of the arm_index arm with step_number step. Args: arm_index (int): Side.RIGHT or Side.LEFT step_number (int): The number of the step. Returns: int: A number that is unique given the step number and arm index. ''' return len(ARM_NAMES) * step_number + arm_index # ################################################################## # Static methods: Internal ("private") # ################################################################## @staticmethod def _make_sphere_marker(uid, pose, frame_id, radius): '''Creates and returns a sphere marker. Args: uid (int): The id to use for the marker. pose (Pose): The pose of the marker. frame_id (str): Frame the marker is associated with. (See std_msgs/Header.msg for more info.) radius (float): Amount to scale the marker by. Scales in all directions (x, y, and z). Returns: Marker ''' return Marker( type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2), scale=Vector3(radius, radius, radius), pose=pose, header=Header(frame_id=frame_id), color=COLOR_TRAJ_ENDPOINT_SPHERES ) @staticmethod def _offset_pose(pose, constant=1): '''Offsets the world pose for visualization. Args: pose (Pose): The pose to offset. constant (int, optional): How much to scale the set offset by (scales ActionStepMarker._offset). Defaults to 1. Returns: Pose: The offset pose. ''' transform = World.get_matrix_from_pose(pose) offset_array = [constant * ActionStepMarker._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices( transform, offset_transform) return World.get_pose_from_transform(hand_transform) # ################################################################## # Instance methods: Public (API) # ################################################################## def get_uid(self): '''Returns a unique id for this marker. Returns: int: A number that is unique given the step number and arm index. ''' return ActionStepMarker.calc_uid(self.arm_index, self.step_number) def decrease_id(self): '''Reduces the step index of the marker.''' self.step_number -= 1 self._update_menu() def update_ref_frames(self, ref_frame_list): '''Updates and re-assigns coordinate frames when the world changes. Args: ref_frame_list ([Object]): List of Object.msg objects, the reference frames of the system. ''' # There is a new list of objects. If the current frame is # relative (already assigned to an object) we need to figure out # the correspondences. ActionStepMarker._ref_object_list = ref_frame_list arm_pose = self.get_target() if arm_pose.refFrame == ArmState.OBJECT: prev_ref_obj = arm_pose.refFrameObject new_ref_obj = World.get_most_similar_obj( prev_ref_obj, ref_frame_list) if new_ref_obj is not None: self.has_object = True arm_pose.refFrameObject = new_ref_obj else: self.has_object = False # Re-populate cached list of reference names. ActionStepMarker._ref_names = [BASE_LINK] for obj in ActionStepMarker._ref_object_list: ActionStepMarker._ref_names.append(obj.name) self._update_menu() def destroy(self): '''Removes marker from the world.''' ActionStepMarker._im_server.erase(self._get_name()) ActionStepMarker._im_server.applyChanges() def set_new_pose(self, new_pose): '''Changes the pose of the action step to new_pose. Args: new_pose (Pose) ''' if self.action_step.type == ActionStep.ARM_TARGET: t = self.action_step.armTarget arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm arm.ee_pose = ActionStepMarker._offset_pose(new_pose, -1) self.update_viz() elif self.action_step.type == ActionStep.ARM_TRAJECTORY: rospy.logwarn( 'Modification of whole trajectory segments is not ' + 'implemented.') def get_absolute_position(self, is_start=True): '''Returns the absolute position of the action step. Args: is_start (bool, optional). For trajectories only. Whether to get the final position in the trajectory. Defaults to True. Returns: Point ''' return self.get_absolute_pose(is_start).position def get_absolute_pose(self, is_start=True): '''Returns the absolute pose of the action step. Args: is_start (bool, optional). For trajectories only. Whether to get the final pose in the trajectory. Defaults to True. Returns: Pose ''' if self.action_step.type == ActionStep.ARM_TARGET: # "Normal" saved pose. t = self.action_step.armTarget arm_pose = t.rArm if self.arm_index == Side.RIGHT else t.lArm elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Trajectory. t = self.action_step.armTrajectory arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm # TODO(mbforbes): Make sure this isn't a bug in the original # implementation. Wouldn't is_start imply you want the first # one? index = len(arm) - 1 if is_start else 0 arm_pose = arm[index] # TODO(mbforbes): Figure out if there are cases that require # this, or remove. # if (arm_pose.refFrame == ArmState.OBJECT and # World.has_object(arm_pose.refFrameObject.name)): # return ActionStepMarker._offset_pose(arm_pose.ee_pose) # else: world_pose = World.get_absolute_pose(arm_pose) return ActionStepMarker._offset_pose(world_pose) def get_pose(self): '''Returns the pose of the action step. Returns: Pose ''' target = self.get_target() if target is not None: return ActionStepMarker._offset_pose(target.ee_pose) def set_target(self, target): '''Sets the new ArmState for this action step. Args: target (ArmState): Replacement for this target. ''' if self.action_step.type == ActionStep.ARM_TARGET: at = self.action_step.armTarget if self.arm_index == Side.RIGHT: at.rArm = target else: at.lArm = target # TODO(mbforbes): Why is self.has_object set to True here? self.has_object = True self._update_menu() self.is_edited = False def get_target(self, traj_index=None): '''Returns the ArmState for this action step. Args: traj_index (int, optional): Which step in the trajectory to return the ArmState for. Only applicable for trajectories (not "normal" saved poses). Defaults to None, in which case the middle point is used. Returns: ArmState ''' if self.action_step.type == ActionStep.ARM_TARGET: t = self.action_step.armTarget return t.rArm if self.arm_index == Side.RIGHT else t.lArm elif self.action_step.type == ActionStep.ARM_TRAJECTORY: t = self.action_step.armTrajectory arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm # If traj_index not passed, use the middle one. if traj_index is None: traj_index = int(len(arm) / 2) return arm[traj_index] def update_viz(self): '''Updates visualization fully.''' self._update_viz_core() self._menu_handler.reApply(ActionStepMarker._im_server) ActionStepMarker._im_server.applyChanges() def pose_reached(self): '''Update when a requested pose is reached.''' self.is_requested = False def change_ref_cb(self, feedback): '''Callback for when a reference frame change is requested. Args: feedback (InteractiveMarkerFeedback (?)) ''' self._menu_handler.setCheckState( self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED) self._menu_handler.setCheckState( feedback.menu_entry_id, MenuHandler.CHECKED) new_ref = self._get_menu_name(feedback.menu_entry_id) self._set_ref(new_ref) rospy.loginfo( 'Switching reference frame to ' + new_ref + ' for action step ' + self._get_name()) self._menu_handler.reApply(ActionStepMarker._im_server) ActionStepMarker._im_server.applyChanges() self.update_viz() def marker_feedback_cb(self, feedback): '''Callback for when an event occurs on the marker. Args: feedback (InteractiveMarkerFeedback) ''' if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.set_new_pose(feedback.pose) self.update_viz() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: # Set the visibility of the 6DOF controller. # This happens a ton, and doesn't need to be logged like # normal events (e.g. clicking on most marker controls # fires here). rospy.logdebug('Changing visibility of the pose controls.') self.is_control_visible = not self.is_control_visible ActionStepMarker._marker_click_cb( self.get_uid(), self.is_control_visible) else: # This happens a ton, and doesn't need to be logged like # normal events (e.g. clicking on most marker controls # fires here). rospy.logdebug('Unknown event: ' + str(feedback.event_type)) # TODO(mbforbes): Figure out types of objects sent to these # callbacks. def delete_step_cb(self, __): '''Callback for when delete is requested. Args: __ (???): Unused ''' self.is_deleted = True def move_to_cb(self, __): '''Callback for when moving to a pose is requested. Args: __ (???): Unused ''' self.is_requested = True def move_pose_to_cb(self, __): '''Callback for when a pose change to current is requested. Args: __ (???): Unused ''' self.is_edited = True # ################################################################## # Instance methods: Internal ("private") # ################################################################## def _is_reachable(self): '''Checks and returns whether there is an IK solution for this action step. Returns: bool: Whether this action step is reachable. ''' dummy, is_reachable = Arms.solve_ik_for_arm( self.arm_index, self.get_target()) # A bit more complicated logging to avoid spamming the logs # while still giving useful info. It now logs when reachability # is first calculated, or changes. report = False # See if we've set reachability for the first time. if self._prev_is_reachable is None: report = True reachable_str = ( 'is reachable' if is_reachable else 'is not reachable') # See if we've got a different reachability than we had before. elif self._prev_is_reachable != is_reachable: report = True reachable_str = ( 'is now reachable' if is_reachable else 'is no longer reachable') # Log if it's changed. if report: rospy.loginfo( 'Pose (' + str(self.step_number) + ', ' + self.arm_name + ') ' + reachable_str) # Cache and return. self._prev_is_reachable = is_reachable return is_reachable def _get_name(self): '''Generates the unique name for the marker. Returns: str: A human-readable unique name for the marker. ''' return 'step' + str(self.step_number) + 'arm' + str(self.arm_index) def _update_menu(self): '''Recreates the menu when something has changed.''' self._menu_handler = MenuHandler() # Insert sub entries. self._sub_entries = [] frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref']) refs = ActionStepMarker._ref_names for ref in refs: subent = self._menu_handler.insert( ref, parent=frame_entry, callback=self.change_ref_cb) self._sub_entries += [subent] # Inset main menu entries. self._menu_handler.insert( MENU_OPTIONS['move_here'], callback=self.move_to_cb) self._menu_handler.insert( MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb) self._menu_handler.insert( MENU_OPTIONS['del'], callback=self.delete_step_cb) # Make all unchecked to start. for subent in self._sub_entries: self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED) # Check if necessary. menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) # Update. self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges() def _get_menu_id(self, ref_name): '''Returns the unique menu id from its name or None if the object is not found. Returns: int (?)|None ''' if ref_name in ActionStepMarker._ref_names: index = ActionStepMarker._ref_names.index(ref_name) return self._sub_entries[index] else: return None def _get_menu_name(self, menu_id): '''Returns the menu name from its unique menu id. Returns: str ''' index = self._sub_entries.index(menu_id) return ActionStepMarker._ref_names[index] def _get_ref_name(self): '''Returns the name string for the reference frame object of the action step. Returns: str|None: Under all normal circumstances, returns the str reference frame name. Returns None in error. ''' ref_name = None if self.action_step.type == ActionStep.ARM_TARGET: # "Normal" step (saved pose). t = self.action_step.armTarget arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm ref_frame = arm.refFrame ref_name = arm.refFrameObject.name elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # "Trajectory" step. t = self.action_step.armTrajectory if self.arm_index == Side.RIGHT: ref_frame = t.rRefFrame ref_name = t.rRefFrameObject.name else: ref_frame = t.lRefFrame ref_name = t.lRefFrameObject.name else: rospy.logerr( 'Unhandled marker type: ' + str(self.action_step.type)) # Update ref frame name if it's absolute. if ref_frame == ArmState.ROBOT_BASE: ref_name = BASE_LINK return ref_name def _set_ref(self, new_ref_name): '''Changes the reference frame of the action step to new_ref_name. Args: new_ref_name ''' # Get the id of the new ref (an int). new_ref = World.get_ref_from_name(new_ref_name) if new_ref != ArmState.ROBOT_BASE: index = ActionStepMarker._ref_names.index(new_ref_name) new_ref_obj = ActionStepMarker._ref_object_list[index - 1] else: new_ref_obj = Object() if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). t = self.action_step.armTarget if self.arm_index == Side.RIGHT: t.rArm = World.convert_ref_frame(t.rArm, new_ref, new_ref_obj) else: t.lArm = World.convert_ref_frame(t.lArm, new_ref, new_ref_obj) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectory steps. t = self.action_step.armTrajectory arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm for i in range(len(arm)): arm_old = arm[i] arm_new = World.convert_ref_frame( arm_old, new_ref, new_ref_obj) arm[i] = arm_new # Fix up reference frames. if self.arm_index == Side.RIGHT: t.rRefFrameObject = new_ref_obj t.rRefFrame = new_ref else: t.lRefFrameObject = new_ref_obj t.lRefFrame = new_ref def _is_hand_open(self): '''Returns whether the gripper is open for this action step. Returns: bool ''' ga = self.action_step.gripperAction gstate = ga.rGripper if self.arm_index == Side.RIGHT else ga.lGripper return gstate.state == GripperState.OPEN def _get_traj_pose(self, index): '''Returns this trajectory's pose at index. Only applicable for trajectories. Args: index (int): Which step in the trajectory to return the pose from. Returns: Pose ''' if self.action_step.type == ActionStep.ARM_TRAJECTORY: at = self.action_step.armTrajectory arm_states = at.rArm if self.arm_index == Side.RIGHT else at.lArm return arm_states[index].ee_pose else: rospy.logerr( 'Cannot request trajectory pose on non-trajectory action ' + 'step.') def _update_viz_core(self): '''Updates visualization after a change.''' # Create a new IM control. menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() # Multiplex marker types added based on action step type. if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). menu_control = self._make_gripper_marker( menu_control, self._is_hand_open()) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectories. # First, get all trajectory positions. point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) # Add a main maker for all points in the trajectory (sphere # list). menu_control.markers.append( Marker( type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_TRAJ_STEP_SPHERES, header=Header(frame_id=frame_id), color=COLOR_TRAJ_STEP_SPHERES, points=point_list ) ) # Add a marker for the first step in the trajectory. menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_FIRST, self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE ) ) # Add a marker for the last step in the trajectory. last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_LAST, self._get_traj_pose(last_index), frame_id, TRAJ_ENDPOINT_SCALE ) ) else: # Neither "normal" pose nor trajectory; error... rospy.logerr( 'Non-handled action step type ' + str(self.action_step.type)) # Add an arrow to the relative object, if there is one. ref_frame = World.get_ref_from_name(frame_id) if ref_frame == ArmState.OBJECT: menu_control.markers.append( Marker( type=Marker.ARROW, id=(ID_OFFSET_REF_ARROW + self.get_uid()), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_OBJ_REF_ARROW, header=Header(frame_id=frame_id), color=COLOR_OBJ_REF_ARROW, points=[pose.position, Point(0, 0, 0)] ) ) # Make and add the text for this step ('Step X'). text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + TEXT_Z_OFFSET menu_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=SCALE_STEP_TEXT, text='Step ' + str(self.step_number), color=COLOR_STEP_TEXT, header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) ) # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert( int_marker, self.marker_feedback_cb) def _add_6dof_marker(self, int_marker, is_fixed): '''Adds a 6 DoF control marker to the interactive marker. Args: int_marker (InteractiveMarker) is_fixed (bool): Looks like whether position is fixed (?). Currently only passed as True. ''' # Each entry in options is (name, orientation, is_move) options = [ ('rotate_x', Quaternion(1, 0, 0, 1), False), ('move_x', Quaternion(1, 0, 0, 1), True), ('rotate_z', Quaternion(0, 1, 0, 1), False), ('move_z', Quaternion(0, 1, 0, 1), True), ('rotate_y', Quaternion(0, 0, 1, 1), False), ('move_y', Quaternion(0, 0, 1, 1), True), ] for opt in options: name, orient, is_move = opt control = self._make_6dof_control(name, orient, is_move, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): '''Creates and returns one component of the 6dof controller. Args: name (str): Name for hte control orientation (Quaternion): How the control should be oriented. is_move (bool): Looks like whether the marker moves the object (?). Currently passed as True for moving markers, False for rotating ones. is_fixed (bool): Looks like whether position is fixed (?). Currently always passed as True. Returns: InteractiveMarkerControl ''' control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if self.is_control_visible: if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control def _make_mesh_marker(self): '''Creates and returns a mesh marker. Returns: Marker ''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = self._get_mesh_marker_color() return mesh def _get_mesh_marker_color(self): '''Gets the color for the mesh marker (thing that looks like a gripper) for this step. A simple implementation of this will return one color for reachable poses, another for unreachable ones. Future implementations may provide further visual cues. Returns: ColorRGBA: The color for the gripper mesh for this step. ''' if self._is_reachable(): return COLOR_MESH_REACHABLE else: return COLOR_MESH_UNREACHABLE def _make_gripper_marker(self, control, is_hand_open=False): '''Makes a gripper marker, adds it to control, returns control. Args: control (InteractiveMarkerControl): IM Control we're using. is_hand_open (bool, optional): Whether the gripper is open. Defaults to False (closed). Returns: InteractiveMarkerControl: The passed control. ''' # Set angle of meshes based on gripper open vs closed. angle = ANGLE_GRIPPER_OPEN if is_hand_open else ANGLE_GRIPPER_CLOSED # Make transforms in preparation for meshes 1, 2, and 3. # NOTE(mbforbes): There are some magic numbers in here that are # used a couple times. Seems like a good candidate for # refactoring to constants, but I think they're more clear if # left in here as (a) they likely won't be changed, and (b) it's # easier to understand the computations with them here. transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) # Create mesh 1 (palm). mesh1 = self._make_mesh_marker() mesh1.mesh_resource = STR_GRIPPER_PALM_FILE mesh1.pose.position.x = -ActionStepMarker._offset mesh1.pose.orientation.w = 1 # Create mesh 2 (finger). mesh2 = self._make_mesh_marker() mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE mesh2.pose = World.get_pose_from_transform(t_proximal) # Create mesh 3 (fingertip). mesh3 = self._make_mesh_marker() mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh3.pose = World.get_pose_from_transform(t_distal) # Make transforms in preparation for meshes 4 and 5. quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle) ) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) # Create mesh 4 (other finger). mesh4 = self._make_mesh_marker() mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE mesh4.pose = World.get_pose_from_transform(t_proximal) # Create mesh 5 (other fingertip). mesh5 = self._make_mesh_marker() mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh5.pose = World.get_pose_from_transform(t_distal) # Append all meshes we made. control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) # Return back the control. # TODO(mbforbes): Why do we do this? return control
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() def get_ee_pose(self): from_frame = '/base_link' to_frame = '/' + self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except: rospy.logwarn('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): rospy.loginfo('You pressed the `Move arm here` button from the menu.') target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_solution = self.ik.get_ik_for_ee(target_pose) #TODO: Send the arms to ik_solution def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1,transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) @staticmethod def _offset_pose(pose): transform = GripperMarkers.get_matrix_from_pose(pose) offset_array = [GripperMarkers._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return GripperMarkers.get_pose_from_transform(hand_transform) @staticmethod def get_matrix_from_pose(pose): rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_solution = self.ik.get_ik_for_ee(target_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
class InteractiveMarkerPoseStampedPublisher(): def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges() def processFeedback(self, feedback): """ :type feedback: InteractiveMarkerFeedback """ s = "Feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" mp = "" if feedback.mouse_point_valid: mp = " at " + str(feedback.mouse_point.x) mp += ", " + str(feedback.mouse_point.y) mp += ", " + str(feedback.mouse_point.z) mp += " in frame " + feedback.header.frame_id if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.logdebug(s + ": button click" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") if feedback.menu_entry_id == 1: rospy.logdebug("Start publishing transform...") if self.tf_br is None: self.tf_br = tf.TransformBroadcaster() elif feedback.menu_entry_id == 2: rospy.logdebug("Stop publishing transform...") self.tf_br = None # When clicking this event triggers! elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.logdebug(s + ": pose changed") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") # TODO: Print here the commands # tf static transform self.print_commands(feedback.pose) self.last_pose = deepcopy(feedback.pose) self.server.applyChanges() def print_commands(self, pose, decimals=4): p = pose.position o = pose.orientation print "\n---------------------------" print "Static transform publisher command (with roll pitch yaw):" common_part = "rosrun tf static_transform_publisher " pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals)) r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Static transform publisher command (with quaternion):" ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Roslaunch line of static transform publisher (rpy):" node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf" roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\ pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />' print roslaunch_part print print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" /> print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />' print "\n---------------------------" def makeArrow(self, msg): marker = Marker() # An arrow that's squashed to easier view the orientation on roll marker.type = Marker.ARROW marker.scale.x = 0.08 marker.scale.y = 0.08 marker.scale.z = 0.08 marker.color.r = 0.3 marker.color.g = 0.3 marker.color.b = 0.5 marker.color.a = 1.0 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeArrow(msg)) msg.controls.append(control) return control def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_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 int_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 int_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 int_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 int_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 int_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 int_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_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name) def run(self): r = pi/2.0 p = pi y = pi/2.0 o = quaternion_from_euler(r, p, y) opt_frame = self.to_frame + "_rgb_optical_frame" r = rospy.Rate(20) while not rospy.is_shutdown(): if self.tf_br is not None: pos = self.last_pose.position ori = self.last_pose.orientation self.tf_br.sendTransform( (pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w), rospy.Time.now(), self.to_frame + "_link", self.from_frame ) ps = PoseStamped() ps.pose = self.last_pose ps.header.frame_id = self.from_frame ps.header.stamp = rospy.Time.now() self.pub.publish(ps) br = tf.TransformBroadcaster() br.sendTransform((0, 0, 0), o, rospy.Time.now(), opt_frame, self.to_frame + "_link") r.sleep()
class ArmStepMarker: ''' Marker for visualizing the steps of an action''' _offset = 0.09 _marker_click_cb = None _reference_change_cb = None def __init__(self, step_number, arm_index, action_step, marker_click_cb, reference_change_cb, parent_sequence): self.action_step = action_step self.arm_index = arm_index self.step_number = step_number self.is_requested = False self.is_deleted = False self.is_control_visible = False self.is_edited = False self.has_object = False self.is_dimmed = False self.is_fake = False self._sub_entries = None self._menu_handler = None ArmStepMarker._marker_click_cb = marker_click_cb ArmStepMarker._reference_change_cb = reference_change_cb self.parent_step_sequence = parent_sequence def _is_reachable(self): '''Checks if there is an IK solution for action step''' dummy, is_reachable = Robot.solve_ik_for_arm(self.arm_index, self.get_target()) # rospy.loginfo('Reachability of pose in ArmStepMarker : ' + # str(is_reachable)) return is_reachable def get_uid(self): '''Returns a unique id of the marker''' return (2 * self.step_number + self.arm_index) def _get_name(self): '''Generates the unique name for the marker''' return ('step' + str(self.step_number) + 'arm' + str(self.arm_index)) def decrease_id(self): '''Reduces the index of the marker''' self.step_number -= 1 self._update_menu() def update_ref_frames(self, ref_frame_list, new_ref_obj): '''Updates and re-assigns coordinate frames when the world changes''' # There is a new list of objects # If the current frames are already assigned to object, # we need to figure out the correspondences self.parent_step_sequence.ref_object_list = ref_frame_list arm_pose = self.get_target() if (arm_pose.refFrame == ArmState.OBJECT): self.has_object = False if (new_ref_obj != None): self.has_object = True arm_pose.refFrameObject = new_ref_obj self.parent_step_sequence.ref_names = ['base_link'] for i in range(len(self.parent_step_sequence.ref_object_list)): self.parent_step_sequence.ref_names.append( self.parent_step_sequence.ref_object_list[i].name) self._update_menu() def destroy(self): '''Removes marker from the world''' self.parent_step_sequence.im_server.erase(self._get_name()) self.parent_step_sequence.im_server.applyChanges() def _update_menu(self): '''Recreates the menu when something has changed''' self._menu_handler = MenuHandler() frame_entry = self._menu_handler.insert('Reference frame') self._sub_entries = [None] * len(self.parent_step_sequence.ref_names) for i in range(len(self.parent_step_sequence.ref_names)): self._sub_entries[i] = self._menu_handler.insert( self.parent_step_sequence.ref_names[i], parent=frame_entry, callback=self.change_ref_cb) self._menu_handler.insert('Move arm here', callback=self.move_to_cb) self._menu_handler.insert('Move to current arm pose', callback=self.move_pose_to_cb) self._menu_handler.insert('Delete', callback=self.delete_step_cb) for i in range(len(self.parent_step_sequence.ref_names)): self._menu_handler.setCheckState(self._sub_entries[i], MenuHandler.UNCHECKED) menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) self._update_viz_core() self._menu_handler.apply(self.parent_step_sequence.im_server, self._get_name()) self.parent_step_sequence.im_server.applyChanges() def _get_menu_id(self, ref_name): '''Returns the unique menu id from its name None if the object is not found''' if ref_name in self.parent_step_sequence.ref_names: index = self.parent_step_sequence.ref_names.index(ref_name) return self._sub_entries[index] else: return None def _get_menu_name(self, menu_id): '''Returns the menu name from its unique menu id''' index = self._sub_entries.index(menu_id) return self.parent_step_sequence.ref_names[index] def _get_ref_name(self): '''Returns the name string for the reference frame object of the action step''' ref_name = None if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: ref_frame = self.action_step.armTarget.rArm.refFrame ref_name = self.action_step.armTarget.rArm.refFrameObject.name else: ref_frame = self.action_step.armTarget.lArm.refFrame ref_name = self.action_step.armTarget.lArm.refFrameObject.name elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): if self.arm_index == 0: ref_frame = self.action_step.armTrajectory.rRefFrame ref_name = self.action_step.armTrajectory.rRefFrameOject.name else: ref_frame = self.action_step.armTrajectory.lRefFrame ref_name = self.action_step.armTrajectory.lRefFrameOject.name else: rospy.logerr('Unhandled marker type: ' + str(self.action_step.type)) if (ref_frame == ArmState.ROBOT_BASE): ref_name = 'base_link' return ref_name def _set_ref(self, new_ref_name): '''Changes the reference frame of the action step''' new_ref = World.get_world().get_ref_from_name(new_ref_name) if (new_ref != ArmState.ROBOT_BASE): index = self.parent_step_sequence.ref_names.index(new_ref_name) new_ref_obj = self.parent_step_sequence.ref_object_list[index - 1] else: new_ref_obj = Object() if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: self.action_step.armTarget.rArm = World.get_world().convert_ref_frame( self.action_step.armTarget.rArm, new_ref, new_ref_obj) else: self.action_step.armTarget.lArm = World.get_world().convert_ref_frame( self.action_step.armTarget.lArm, new_ref, new_ref_obj) elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): for i in range(len(self.action_step.armTrajectory.timing)): if self.arm_index == 0: arm_old = self.action_step.armTrajectory.rArm[i] arm_new = World.get_world().convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.rArm[i] = arm_new else: arm_old = self.action_step.armTrajectory.lArm[i] arm_new = World.get_world().convert_ref_frame(arm_old, new_ref, new_ref_obj) self.action_step.armTrajectory.lArm[i] = arm_new if self.arm_index == 0: self.action_step.armTrajectory.rRefFrameObject = new_ref_obj self.action_step.armTrajectory.rRefFrame = new_ref else: self.action_step.armTrajectory.lRefFrameObject = new_ref_obj self.action_step.armTrajectory.lRefFrame = new_ref ArmStepMarker._reference_change_cb(self.get_uid(), new_ref, new_ref_obj) def _is_hand_open(self): '''Checks if the gripper is open for the action step''' if self.arm_index == 0: gripper_state = self.action_step.gripperAction.rGripper else: gripper_state = self.action_step.gripperAction.lGripper return (gripper_state == GripperState.OPEN) def set_new_pose(self, new_pose): '''Changes the pose of the action step''' if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: pose = ArmStepMarker._offset_pose(new_pose, -1) self.action_step.armTarget.rArm.ee_pose = pose else: pose = ArmStepMarker._offset_pose(new_pose, -1) self.action_step.armTarget.lArm.ee_pose = pose rospy.loginfo('Set new pose for action step.') self.update_viz() elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): rospy.logwarn('Modification of whole trajectory ' + 'segments is not implemented.') def get_absolute_position(self, is_start=True): '''Returns the absolute position of the action step''' pose = self.get_absolute_pose(is_start) return pose.position def get_absolute_pose(self, is_start=True): '''Returns the absolute pose of the action step''' if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: arm_pose = self.action_step.armTarget.rArm else: arm_pose = self.action_step.armTarget.lArm elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): if self.arm_index == 0: if is_start: index = len(self.action_step.armTrajectory.rArm) - 1 arm_pose = self.action_step.armTrajectory.rArm[index] else: arm_pose = self.action_step.armTrajectory.rArm[0] else: if is_start: index = len(self.action_step.armTrajectory.lArm) - 1 arm_pose = self.action_step.armTrajectory.lArm[index] else: arm_pose = self.action_step.armTrajectory.lArm[0] #if (arm_pose.refFrame == ArmState.OBJECT and # World.has_object(arm_pose.refFrameObject.name)): # return ArmStepMarker._offset_pose(arm_pose.ee_pose) #else: world_pose = World.get_world().get_absolute_pose(arm_pose) return ArmStepMarker._offset_pose(world_pose) def get_pose(self): '''Returns the pose of the action step''' target = self.get_target() if (target != None): return ArmStepMarker._offset_pose(target.ee_pose) @staticmethod def _offset_pose(pose, constant=1): '''Offsets the world pose for visualization''' transform = World.get_world().get_matrix_from_pose(pose) offset_array = [constant * ArmStepMarker._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return World.get_world().get_pose_from_transform(hand_transform) def set_target(self, target): '''Sets the new pose for the action step''' if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: self.action_step.armTarget.rArm = target else: self.action_step.armTarget.lArm = target self.has_object = True self._update_menu() self.is_edited = False def get_target(self, traj_index=None): '''Returns the pose for the action step''' if (self.action_step.type == ArmStepType.ARM_TARGET): if self.arm_index == 0: return self.action_step.armTarget.rArm else: return self.action_step.armTarget.lArm elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): if self.arm_index == 0: if traj_index is None: traj = self.action_step.armTrajectory.rArm traj_index = int(len(traj) / 2) return self.action_step.armTrajectory.rArm[traj_index] else: if traj_index is None: traj = self.action_step.armTrajectory.lArm traj_index = int(len(traj) / 2) return self.action_step.armTrajectory.lArm[traj_index] def _get_traj_pose(self, index): '''Returns a single pose for trajectory''' if (self.action_step.type == ArmStepType.ARM_TRAJECTORY): if self.arm_index == 0: target = self.action_step.armTrajectory.rArm[index] else: target = self.action_step.armTrajectory.lArm[index] return target.ee_pose else: rospy.logerr('Cannot request trajectory pose ' + 'on non-trajectory action step.') def _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ArmStepType.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ArmStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ArmStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_world().get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): # The following is needed to properly display the arrtow in browser, due to the fact that ros3djs # displays all nested markers in the reference frame of the interactive marker. # Thus, we need to calculate the position of the object in the reference frame of the interactive marker. quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] inv_quat_matrix = quaternion_matrix(quaternion_inverse(quat)) pose_vec = numpy.array((pose.position.x, pose.position.y, pose.position.z, 0)) new_pose = numpy.dot(inv_quat_matrix, pose_vec) menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.01, 0.01, 0.0001), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.8, 0.0, 0.6), points=[Point(0, 0, 0), Point(-new_pose[0], -new_pose[1], -new_pose[2])])) # Calculate text position so that they "orbit" around the marker; # this is done so that poses in identical or similar positions # have non-overlapping text. Note that to do this without moving # the text around as the camera is moved, we assume that the viewer # is always looking directly at the robot, so we assume the x dimension # is constant and "orbin" in the y-z plane. n_orbitals = 8 # this should be a constant offset = 0.15 # this should be a constant orbital = (self.step_number - 1) % n_orbitals # - 1 to make 0-based angle_rad = (float(orbital) / n_orbitals) * (-2 * numpy.pi) + \ (numpy.pi / 2.0) # start above, at pi/2 (90 degrees) text_pos = Point() text_pos.x = 0 text_pos.y = numpy.cos(angle_rad) * offset text_pos.z = numpy.sin(angle_rad) * offset r,g,b = self.get_marker_color() menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0.06, 0.06, 0.06), lifetime=rospy.Duration(1.5), text='Step ' + str(self.step_number), color=ColorRGBA(r, g, b, 1.0), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) self.parent_step_sequence.im_server.insert(int_marker, self.marker_feedback_cb) @staticmethod def make_sphere_marker(uid, pose, frame_id, radius): '''Creates a sphere marker''' return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2), scale=Vector3(radius, radius, radius), pose=pose, header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.5, 0.0, 0.8)) def marker_feedback_cb(self, feedback): '''Callback for when an event occurs on the marker''' if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.set_new_pose(feedback.pose) self.update_viz() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: # Set the visibility of the 6DOF controller rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True ArmStepMarker._marker_click_cb(self.get_uid(), self.is_control_visible) else: rospy.loginfo('Unknown event' + str(feedback.event_type)) def delete_step_cb(self, dummy): '''Callback for when delete is requested''' self.is_deleted = True def move_to_cb(self, dummy): '''Callback for when moving to a pose is requested''' self.is_requested = True def move_pose_to_cb(self, dummy): '''Callback for when a pose change to current is requested''' self.is_edited = True def pose_reached(self): '''Update when a requested pose is reached''' self.is_requested = False def change_ref_cb(self, feedback): '''Callback for when a reference frame change is requested''' self._menu_handler.setCheckState( self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED) self._menu_handler.setCheckState(feedback.menu_entry_id, MenuHandler.CHECKED) new_ref = self._get_menu_name(feedback.menu_entry_id) self._set_ref(new_ref) rospy.loginfo('Switching reference frame to ' + new_ref + ' for action step ' + self._get_name()) self._menu_handler.reApply(self.parent_step_sequence.im_server) self.parent_step_sequence.im_server.applyChanges() self.update_viz() def update_viz(self): '''Updates visualization fully''' self._update_viz_core() self._menu_handler.reApply(self.parent_step_sequence.im_server) self.parent_step_sequence.im_server.applyChanges() def _add_6dof_marker(self, int_marker, is_fixed): '''Adds a 6 DoF control marker to the interactive marker''' control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): '''Creates one component of the 6dof controller''' control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control def get_marker_color(self): '''Makes marker colors in a gradient according to the progression set in _get_rgb_from_abs_pos() (below). returns (r,g,b) tuple, each from 0.0 to 1.0 ''' total = self.parent_step_sequence.total_n_markers # These are 1-based indexing; turn them into 0-based indexing. idx = self.step_number - 1 # First calculate "absolute" color position, plotting on a 0.0 - 1.0 # scale. This only applies if there's more than one step; otherwise we # set to 0.0 (though it could be anything, as it just gets multiplied # by 0, as the idx must be 0 if there's only one step total). abs_step_size = 1.0 / float(total - 1) if total > 1 else 0.0 abs_pos = abs_step_size * idx # Then, use our helper method to turn this into an RGB value. return self._get_rgb_from_abs_pos(abs_pos) def _get_rgb_from_abs_pos(self, abs_pos): '''Turns abs_pos, a float from 0.0 to 1.0 inclusive, into an rgb value of the range currently programmed. The progression is as follows (in order to avoid green hues, which could be confused with the objects), by gradient "bucket" step: - 0 yellow (start) -> red - 1 red -> purple - 2 purple -> blue - 3 blue -> cyan (end) Returns (r,g,b) tuple of floats, each from 0.0 to 1.0 inclusive.''' # Bucket settings (make constant) bucket = self.arm_index bucket_pos = abs_pos # Now translate to colors; todo later implement with better data # structure. r = 0.0 g = 0.0 b = 0.0 if bucket == 0: # yellow -> red r = 1.0 g = 1.0 - bucket_pos elif bucket == 1: # cyan -> blue g = 1.0 - bucket_pos b = 1.0 else: # Set white as error color rospy.logwarn("Bad color gradient; bucket " + str(bucket) + " and bucket position " + str(bucket_pos)) r,g,b = 1.0, 1.0, 1.0 return (r,g,b) def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 alpha = 0.6 if self.is_dimmed: alpha = 0.1 if self.is_fake: alpha = 0.4 mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha) return mesh if self._is_reachable(): # Original: some kinda orange # r,g,b = 1.0, 0.5, 0.0 # New: rainbow! See method comment for details. r,g,b = self.get_marker_color() mesh.color = ColorRGBA(r, g, b, alpha) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha) return mesh def _make_gripper_marker(self, control, is_hand_open=False): '''Makes a gripper marker''' if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -ArmStepMarker._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger.dae') mesh2.pose = World.get_world().get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger_tip.dae') mesh3.pose = World.get_world().get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger.dae') mesh4.pose = World.get_world().get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/' + 'gripper_v0/l_finger_tip.dae') mesh5.pose = World.get_world().get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control