class FakeMarkerServer(): def __init__(self): # create a simple TF listener self.tf_listener = tf.TransformListener() self.grasp_check = rospy.ServiceProxy( '/interactive_world_hackathon/grasp_check', GraspCheck) self.speak = rospy.ServiceProxy('/tts/speak', Speak) self.play = rospy.ServiceProxy('/tts/play', Speak) # create the nav client self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction) self.nav.wait_for_server() # create the backup client self.backup = actionlib.SimpleActionClient('backup_action', BackupAction) self.backup.wait_for_server() # create the place action client self.place = actionlib.SimpleActionClient( 'object_manipulator/object_manipulator_place', PlaceAction) self.place.wait_for_server() # Segmentation client self.segclient = actionlib.SimpleActionClient( '/object_detection_user_command', UserCommandAction) self.segclient.wait_for_server() self.recognition = None # create the IMGUI action client self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction) self.imgui.wait_for_server() # listen for graspable objects rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list) # create the save service rospy.Service('~save_template', SaveTemplate, self.save) self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False) self.load_server.start() # create the IM server self.server = InteractiveMarkerServer('~fake_marker_server') # create return list of templates rospy.Service('~print_templates', PrintTemplates, self.get_templates) # used to get model meshes self.get_mesh = rospy.ServiceProxy( '/objects_database_node/get_model_mesh', GetModelMesh) # hack to get the grasp rospy.Subscriber( '/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp) self.last_grasp = None self.objects = [] self.objects.append(OBJECT1) self.objects.append(OBJECT2) self.objects.append(OBJECT3) self.reset_objects() # check for saved templates try: self.templates = pickle.load(open(SAVE_FILE, 'rb')) rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).') except: self.templates = dict() rospy.loginfo('New template file started.') self.play( '/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav') #Service: rosservice call /fake_object_markers/print_templates #Returns a string of template names #ex) list: ['test_template1','test_template2','test_template3'] def get_templates(self, req): temp_list = [] if self.templates.keys() is None: self.publish_feedback('No templates') return for obj in self.templates.keys(): temp_list.append(str(obj)) #print temp_list return PrintTemplatesResponse(temp_list) def store_grasp(self, msg): self.last_grasp = msg.result.grasp # Given a mesh_id creates a name with format 'object + mesh_id' # ex.)Given '1234', creates 'object_1234' name def create_name(self, mesh_id): return 'object_' + str(mesh_id) # Creates a mesh of the given object with the given pose to be visualized by template maker def create_mesh(self, mesh_id, pose): response = self.get_mesh(mesh_id) mesh = response.mesh # build the mesh marker marker = Marker() marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 marker.frame_locked = False marker.type = Marker.TRIANGLE_LIST # add the mesh for j in range(len(mesh.triangles)): marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[0]]) marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[1]]) marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[2]]) # create the interactive marker name = self.create_name(mesh_id) self.server.insert(self.create_im(marker, pose, name), self.process_feedback) self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # Creates an interactive marker # - at the given location and pose # - with a given name # - for given marker object def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker def process_feedback(self, feedback): self.last_feedback = feedback # Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range def check_pose(self, x, y): return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END # Checks position of hallucinated interactive markers # Changes color and sets position when user releases mouse click (MOUSE_UP) on object def release(self, feedback): im = self.server.get(feedback.marker_name) # copy the mesh information marker = copy.deepcopy(im.controls[0].markers[0]) # determine color based on pose if self.check_pose(feedback.pose.position.x, feedback.pose.position.y): marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.66 else: marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 # create the new interactive marker self.server.insert( self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback) self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # updates server def update(self): self.server.applyChanges() # **Run by save_template service** # Saves given template information to file location def save(self, req): # go through each object and check if they are in the valid range to_save = [] for obj_id in self.objects: im = self.server.get(self.create_name(obj_id)) pose = im.pose if (self.check_pose(pose.position.x, pose.position.y)): to_save.append(copy.deepcopy(im)) # check if we have anything to save if len(to_save) > 0: self.templates[req.name] = to_save # PICKLE IT! pickle.dump(self.templates, open(SAVE_FILE, 'wb')) self.play('/home/rctoris/wav/t3_affirmative.wav') self.reset_objects() return SaveTemplateResponse(True) else: return SaveTemplateResponse(False) # Publishes feedback of current tasks def publish_feedback(self, msg): rospy.loginfo(msg) self.load_server.publish_feedback(LoadFeedback(msg)) # Publishes final result of action def publish_result(self, msg): rospy.loginfo(msg) self.load_server.set_succeeded(LoadResult(msg)) # Returns how many objects were recognized def proc_grasp_list(self, msg): objects = [] # start by going through each size = len(msg.graspable_objects) for i in range(size): obj = msg.graspable_objects[i] # only take recognized objects if len(obj.potential_models) is not 0: objects.append(copy.deepcopy(obj)) rospy.loginfo('Found ' + str(len(objects)) + ' object(s).') self.recognition = objects # Drives to and aligns with counter # Segments objects def look_for_objects(self): self.publish_feedback('Driving robot to counter') # drive the robot nav_goal = navigateGoal('Snack Nav', True) self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Counter alignment failed.') return False self.publish_feedback('Aligned robot to counter') self.publish_feedback('Looking for objects') self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav') self.recognition = None # Segment the table self.segclient.send_goal(UserCommandGoal(request=1, interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) # Recognize objects self.recognition = None self.segclient.send_goal(UserCommandGoal(request=2, interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) return True # **Run by load_template service** # Identifies remaining objects needed in template # Moves to and aligns with counter # Scans and recognizes objects on counter that match template # Picks up one object # Backs up from counter # Drives to table # Places object in given template location # Repeats def load(self, goal): name = goal.name self.publish_feedback('Loading template ' + name) # if requested template does not exist... if name not in self.templates.keys(): self.publish_result(name + ' template does not exist') return template = copy.deepcopy(self.templates[name]) self.publish_feedback('Loaded template ' + name) self.play('/home/rctoris/wav/help.wav') # look for any objects we need while len(template) is not 0: pickup_arm = None # if it does not see any objects/could not drive to counter if not self.look_for_objects(): self.publish_result('Object looking failed.') return # for each object in template array... for template_im in template: # for each recognized object for rec_obj in self.recognition: if template_im.name == self.create_name( rec_obj.potential_models[0].model_id): # create the object info for it obj_info = self.create_object_info(rec_obj) # pick it up pickup_arm = self.pickup(rec_obj) # if neither arm can could pick up object... if pickup_arm is None: self.publish_result('Pickup failed.') return # make sure we have a grasp self.publish_feedback('Waiting for grasp') while self.last_grasp is None: rospy.sleep(1) # store the grasp obj_info.grasp = self.last_grasp self.last_grasp = None self.publish_feedback('Grasp found') # good job robot, place that object to_place = Pose() # location of object in template on table to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET to_place.position.x = template_im.pose.position.x to_place.position.y = template_im.pose.position.y placed = self.place_object(obj_info, pickup_arm, to_place) # if the object could not be placed if not placed: self.publish_result('Place failed.') return self.publish_feedback('Placed the object!') if len(template) is not 1: self.play('/home/rctoris/wav/ill-be-back.wav') # removes object from list of objects to pick up from template template.remove(template_im) # if no objects are found... if pickup_arm is None: # No objects found :( self.publish_result('No objects found that we need :(') return # We completed the task! self.play('/home/rctoris/wav/down.wav') self.publish_result('Great success!') # resets collision map of world and rescan def reset_collision_map(self): self.publish_feedback('Reseting collision map') goal = IMGUIGoal() goal.command.command = 3 goal.options.reset_choice = 4 self.imgui.send_goal(goal) self.imgui.wait_for_result() self.publish_feedback('Collision map reset') # reset hallucinated interactive marker objects' positions on visualized table def reset_objects(self): pose = Pose() pose.position.z = TABLE_HEIGHT pose.position.x = OFFSET * 3 pose.position.y = -0.25 for obj_id in self.objects: self.create_mesh(obj_id, pose) pose.position.y = pose.position.y + 0.25 # Picks up object that matches obj def pickup(self, obj): # start by picking up the object options = IMGUIOptions() options.collision_checked = True options.grasp_selection = 1 options.adv_options.lift_steps = 10 options.adv_options.retreat_steps = 10 options.adv_options.reactive_force = False options.adv_options.reactive_grasping = False options.adv_options.reactive_place = False options.adv_options.lift_direction_choice = 0 # check which arm is closer if obj.potential_models[0].pose.pose.position.y > 0: options.arm_selection = 1 else: options.arm_selection = 0 goal = IMGUIGoal() goal.options = options goal.options.grasp_selection = 1 goal.options.selected_object = obj goal.command.command = goal.command.PICKUP # send it to IMGUI self.publish_feedback('Attempting to pick up') self.reset_collision_map() self.imgui.send_goal(goal) self.play('/home/rctoris/wav/humnbehv.wav') self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try the other arm if options.arm_selection is 0: options.arm_selection = 1 else: options.arm_selection = 0 self.publish_feedback('Initial pickup failed, trying other arm') self.reset_collision_map() self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: return None else: # now check if feedback to see if we actually got it if options.arm_selection is 0: arm = 'right' else: arm = 'left' self.publish_feedback('Checking if object was grasped') resp = self.grasp_check(arm) if resp.isGrasping is True: self.publish_feedback('Object was grasped') # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) res = self.backup.get_result() # if robot could not back up if not res.success: self.publish_feedback('Backup failed.') return None return options.arm_selection else: self.move_arm_to_side(options.arm_selection) return None # moves arm to sides def move_arm_to_side(self, arm_selection): goal = IMGUIGoal() goal.command.command = 4 goal.options.arm_selection = arm_selection goal.options.arm_action_choice = 0 goal.options.arm_planner_choice = 1 self.publish_feedback('Moving arm to the side using planner') self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try open loop self.publish_feedback('Planned arm move failed, trying open loop') goal.options.arm_planner_choice = 0 self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: self.publish_feedback('Arm move failed.') return False else: self.publish_feedback('Arm move was successful') return True # place object in given arm to given pose def place_object(self, obj_info_orig, arm_selection, pose): #drive to the table self.publish_feedback('Driving robot to table') nav_goal = navigateGoal('Dining Table Nav', True) self.play('/home/rctoris/wav/run.wav') self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Table alignment failed.') return False self.publish_feedback('Aligned robot to table') self.reset_collision_map() self.publish_feedback('Attempting to place the object') # make a copy obj_info = copy.deepcopy(obj_info_orig) obj = obj_info.obj goal = PlaceGoal() # set the arm if arm_selection is 0: goal.arm_name = 'right_arm' prefix = 'r' else: goal.arm_name = 'left_arm' prefix = 'l' # rotate and "gu-chunk" orig_z = pose.position.z pose.orientation.x = 0 pose.orientation.y = 0 goal.place_locations = [] #iterating through possible x-locations to place object for x in range(0, 10): pose.position.x = pose.position.x + ((x - 5) * 0.0025) #iterating through possible y-locations to place object for y in range(0, 10): pose.position.y = pose.position.y + ((y - 5) * 0.0025) # 'i' is for some rotations for i in range(0, 10): rads = (pi * (i / 10.0)) pose.orientation.z = sin(-rads / 2.0) pose.orientation.w = cos(-rads / 2.0) # 'j' is for the 'z' height for j in range(0, 6): pose.position.z = orig_z + (j * 0.0025) pose_mat = pose_to_mat(pose) to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose) gripper_mat = to_base_link_mat * grasp_mat gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link') goal.place_locations.append(gripper_pose) # use the identity as the grasp obj_info.grasp.grasp_pose.pose = Pose() obj_info.grasp.grasp_pose.pose.orientation.w = 1 goal.grasp = obj_info.grasp goal.desired_retreat_distance = 0.1 goal.min_retreat_distance = 0.05 # set the approach goal.approach = GripperTranslation() goal.approach.desired_distance = .1 goal.approach.min_distance = 0.05 goal.approach.direction = create_vector3_stamped([0., 0., -1.], 'base_link') # set the collision info goal.collision_object_name = obj.collision_name goal.collision_support_surface_name = 'table' goal.place_padding = 0.0 goal.additional_link_padding = self.create_gripper_link_padding(prefix) goal.collision_support_surface_name = 'all' goal.allow_gripper_support_collision = True goal.use_reactive_place = False # send the goal self.place.send_goal(goal) # wait for result finished_within_time = self.place.wait_for_result(rospy.Duration(240)) if not finished_within_time: self.place.cancel_goal() return False # check the result res = self.place.get_result() if res.manipulation_result.value == -6 or res.manipulation_result.value == 1: # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) backup_res = self.backup.get_result() # if robot could not back up if not backup_res.success: self.publish_feedback('Backup failed.') return False self.move_arm_to_side(arm_selection) return True else: return False def create_gripper_link_padding(self, prefix): link_name_list = [ '_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link', '_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link' ] pad = 0. arm_link_names = [prefix + link_name for link_name in link_name_list] link_padding_list = [ LinkPadding(link_name, pad) for link_name in arm_link_names ] return link_padding_list def create_object_info(self, obj): # get the pose pose_stamped = obj.potential_models[0].pose # change the frame obj_frame_pose_stamped = change_pose_stamped_frame( self.tf_listener, pose_stamped, obj.reference_frame_id) return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class AnnotatorServer(object): def __init__(self): self._annotator = Annotator() self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names", PoseNames, queue_size=10, latch=True) # self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses", # InteractiveMarker, queue_size=10, latch=True) self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses") self.INITIAL_POSE = Pose() self.INITIAL_POSE.orientation.w = 1 print("Initializing saved markers: " + str(self._annotator.get_position_names())) for name, pose in self._annotator.get_position_items(): self.__create_int_marker__(name, pose) self.__pub_pose_info__() print("Initialization finished...") def __pub_pose_info__(self): pose_names = PoseNames() pose_names.names = self._annotator.get_position_names() pose_names.names.sort() self._pose_names_pub.publish(pose_names) def __update_marker_pose__(self, input): if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP): name = input.marker_name new_pose = self._int_marker_server.get(name).pose # Overwrite the previous pose with the new pose self._annotator.save_position(name, new_pose) self._int_marker_server.setPose(name, new_pose) self._int_marker_server.applyChanges() print("updated pose for: " + name) def __create_int_marker__(self, name, pose): print("creating int marker: " + name) int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = name int_marker.description = name int_marker.pose = pose # Move it 0.25 meters up to make it easier to click int_marker.pose.position.z = 0.25 text_marker = Marker() text_marker.text = name text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.position.z = 2 text_marker.scale.x = 0.4 text_marker.scale.y = 0.4 text_marker.scale.z = 0.4 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1.0 text_control = InteractiveMarkerControl() text_control.name = "text_control" text_control.markers.append(text_marker) text_control.always_visible = True text_control.interaction_mode = InteractiveMarkerControl.NONE int_marker.controls.append(text_control) rotation_ring_control = InteractiveMarkerControl() rotation_ring_control.name = "position_control" rotation_ring_control.always_visible = True rotation_ring_control.orientation.x = 0 rotation_ring_control.orientation.w = 1 rotation_ring_control.orientation.y = 1 rotation_ring_control.orientation.z = 0 rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(rotation_ring_control) arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.pose.position.z = 0.15 arrow_marker.pose.position.x = -0.5 arrow_marker.scale.x = 1 arrow_marker.scale.y = 0.25 arrow_marker.scale.z = 0.25 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 position_control = InteractiveMarkerControl() position_control.name = "rotation_control" position_control.always_visible = True position_control.markers.append(arrow_marker) position_control.orientation.w = 1 position_control.orientation.x = 0 position_control.orientation.y = 1 position_control.orientation.z = 0 position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(position_control) self._int_marker_server.insert(int_marker, self.__update_marker_pose__) self._int_marker_server.applyChanges() def create(self, name, pose=None): print("creating new pose: " + name) if pose is None: pose = self.INITIAL_POSE self._annotator.save_position(name, pose) self.__create_int_marker__(name, pose) self.__pub_pose_info__() def delete(self, name): if self._annotator.exists(name): print("deleting pose: " + name) self._annotator.delete_position(name) self._int_marker_server.erase(name) self._int_marker_server.applyChanges() self.__pub_pose_info__() def handle_callback(self, user_action_msg): cmd = user_action_msg.command name = user_action_msg.name if cmd == UserAction.CREATE: self.create(name) elif cmd == UserAction.DELETE: self.delete(name) elif cmd == UserAction.GOTO: print("going to pose: " + name) self._annotator.goto_position(name)
class FakeMarkerServer(): def __init__(self): # create a simple TF listener self.tf_listener = tf.TransformListener() self.grasp_check = rospy.ServiceProxy('/interactive_world_hackathon/grasp_check', GraspCheck) self.speak = rospy.ServiceProxy('/tts/speak', Speak) self.play = rospy.ServiceProxy('/tts/play', Speak) # create the nav client self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction) self.nav.wait_for_server() # create the backup client self.backup = actionlib.SimpleActionClient('backup_action', BackupAction) self.backup.wait_for_server() # create the place action client self.place = actionlib.SimpleActionClient('object_manipulator/object_manipulator_place', PlaceAction) self.place.wait_for_server() # Segmentation client self.segclient = actionlib.SimpleActionClient('/object_detection_user_command', UserCommandAction) self.segclient.wait_for_server() self.recognition=None # create the IMGUI action client self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction) self.imgui.wait_for_server() # listen for graspable objects rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list) # create the save service rospy.Service('~save_template', SaveTemplate, self.save) self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False) self.load_server.start() # create the IM server self.server = InteractiveMarkerServer('~fake_marker_server') # create return list of templates rospy.Service('~print_templates', PrintTemplates, self.get_templates) # used to get model meshes self.get_mesh = rospy.ServiceProxy('/objects_database_node/get_model_mesh', GetModelMesh) # hack to get the grasp rospy.Subscriber('/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp) self.last_grasp = None self.objects = [] self.objects.append(OBJECT1) self.objects.append(OBJECT2) self.objects.append(OBJECT3) self.reset_objects() # check for saved templates try: self.templates = pickle.load(open(SAVE_FILE, 'rb')) rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).') except: self.templates = dict() rospy.loginfo('New template file started.') self.play('/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav') #Service: rosservice call /fake_object_markers/print_templates #Returns a string of template names #ex) list: ['test_template1','test_template2','test_template3'] def get_templates(self, req): temp_list = [] if self.templates.keys() is None: self.publish_feedback('No templates') return for obj in self.templates.keys(): temp_list.append(str(obj)) #print temp_list return PrintTemplatesResponse(temp_list) def store_grasp(self, msg): self.last_grasp = msg.result.grasp # Given a mesh_id creates a name with format 'object + mesh_id' # ex.)Given '1234', creates 'object_1234' name def create_name(self, mesh_id): return 'object_' + str(mesh_id) # Creates a mesh of the given object with the given pose to be visualized by template maker def create_mesh(self, mesh_id, pose): response = self.get_mesh(mesh_id) mesh = response.mesh # build the mesh marker marker = Marker() marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 marker.frame_locked = False marker.type = Marker.TRIANGLE_LIST # add the mesh for j in range(len(mesh.triangles)): marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]]) marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]]) marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]]) # create the interactive marker name = self.create_name(mesh_id) self.server.insert(self.create_im(marker, pose, name), self.process_feedback) self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # Creates an interactive marker # - at the given location and pose # - with a given name # - for given marker object def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker def process_feedback(self, feedback): self.last_feedback = feedback # Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range def check_pose(self, x, y): return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END # Checks position of hallucinated interactive markers # Changes color and sets position when user releases mouse click (MOUSE_UP) on object def release(self, feedback): im = self.server.get(feedback.marker_name) # copy the mesh information marker = copy.deepcopy(im.controls[0].markers[0]) # determine color based on pose if self.check_pose(feedback.pose.position.x, feedback.pose.position.y): marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.66 else: marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 # create the new interactive marker self.server.insert(self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback) self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # updates server def update(self): self.server.applyChanges() # **Run by save_template service** # Saves given template information to file location def save(self, req): # go through each object and check if they are in the valid range to_save = [] for obj_id in self.objects: im = self.server.get(self.create_name(obj_id)) pose = im.pose if (self.check_pose(pose.position.x, pose.position.y)): to_save.append(copy.deepcopy(im)) # check if we have anything to save if len(to_save) > 0: self.templates[req.name] = to_save # PICKLE IT! pickle.dump(self.templates, open(SAVE_FILE, 'wb')) self.play('/home/rctoris/wav/t3_affirmative.wav') self.reset_objects() return SaveTemplateResponse(True) else: return SaveTemplateResponse(False) # Publishes feedback of current tasks def publish_feedback(self, msg): rospy.loginfo(msg) self.load_server.publish_feedback(LoadFeedback(msg)) # Publishes final result of action def publish_result(self, msg): rospy.loginfo(msg) self.load_server.set_succeeded(LoadResult(msg)) # Returns how many objects were recognized def proc_grasp_list(self, msg): objects = [] # start by going through each size = len(msg.graspable_objects) for i in range(size): obj = msg.graspable_objects[i] # only take recognized objects if len(obj.potential_models) is not 0: objects.append(copy.deepcopy(obj)) rospy.loginfo('Found ' + str(len(objects)) + ' object(s).') self.recognition = objects # Drives to and aligns with counter # Segments objects def look_for_objects(self): self.publish_feedback('Driving robot to counter') # drive the robot nav_goal = navigateGoal('Snack Nav', True) self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Counter alignment failed.') return False self.publish_feedback('Aligned robot to counter') self.publish_feedback('Looking for objects') self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav') self.recognition = None # Segment the table self.segclient.send_goal(UserCommandGoal(request=1,interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) # Recognize objects self.recognition = None self.segclient.send_goal(UserCommandGoal(request=2,interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) return True # **Run by load_template service** # Identifies remaining objects needed in template # Moves to and aligns with counter # Scans and recognizes objects on counter that match template # Picks up one object # Backs up from counter # Drives to table # Places object in given template location # Repeats def load(self, goal): name = goal.name self.publish_feedback('Loading template ' + name) # if requested template does not exist... if name not in self.templates.keys(): self.publish_result(name + ' template does not exist') return template = copy.deepcopy(self.templates[name]) self.publish_feedback('Loaded template ' + name) self.play('/home/rctoris/wav/help.wav') # look for any objects we need while len(template) is not 0: pickup_arm = None # if it does not see any objects/could not drive to counter if not self.look_for_objects(): self.publish_result('Object looking failed.') return # for each object in template array... for template_im in template: # for each recognized object for rec_obj in self.recognition: if template_im.name == self.create_name(rec_obj.potential_models[0].model_id): # create the object info for it obj_info = self.create_object_info(rec_obj) # pick it up pickup_arm = self.pickup(rec_obj) # if neither arm can could pick up object... if pickup_arm is None: self.publish_result('Pickup failed.') return # make sure we have a grasp self.publish_feedback('Waiting for grasp') while self.last_grasp is None: rospy.sleep(1) # store the grasp obj_info.grasp = self.last_grasp self.last_grasp = None self.publish_feedback('Grasp found') # good job robot, place that object to_place = Pose() # location of object in template on table to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET to_place.position.x = template_im.pose.position.x to_place.position.y = template_im.pose.position.y placed = self.place_object(obj_info, pickup_arm, to_place) # if the object could not be placed if not placed: self.publish_result('Place failed.') return self.publish_feedback('Placed the object!') if len(template) is not 1: self.play('/home/rctoris/wav/ill-be-back.wav') # removes object from list of objects to pick up from template template.remove(template_im) # if no objects are found... if pickup_arm is None: # No objects found :( self.publish_result('No objects found that we need :(') return # We completed the task! self.play('/home/rctoris/wav/down.wav') self.publish_result('Great success!') # resets collision map of world and rescan def reset_collision_map(self): self.publish_feedback('Reseting collision map') goal = IMGUIGoal() goal.command.command = 3 goal.options.reset_choice = 4 self.imgui.send_goal(goal) self.imgui.wait_for_result() self.publish_feedback('Collision map reset') # reset hallucinated interactive marker objects' positions on visualized table def reset_objects(self): pose = Pose() pose.position.z = TABLE_HEIGHT pose.position.x = OFFSET * 3 pose.position.y = -0.25 for obj_id in self.objects: self.create_mesh(obj_id, pose) pose.position.y = pose.position.y + 0.25 # Picks up object that matches obj def pickup(self, obj): # start by picking up the object options = IMGUIOptions() options.collision_checked = True options.grasp_selection = 1 options.adv_options.lift_steps = 10 options.adv_options.retreat_steps = 10 options.adv_options.reactive_force = False options.adv_options.reactive_grasping = False options.adv_options.reactive_place = False options.adv_options.lift_direction_choice = 0 # check which arm is closer if obj.potential_models[0].pose.pose.position.y > 0: options.arm_selection = 1 else: options.arm_selection = 0 goal = IMGUIGoal() goal.options = options goal.options.grasp_selection = 1 goal.options.selected_object = obj goal.command.command = goal.command.PICKUP # send it to IMGUI self.publish_feedback('Attempting to pick up') self.reset_collision_map() self.imgui.send_goal(goal) self.play('/home/rctoris/wav/humnbehv.wav') self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try the other arm if options.arm_selection is 0: options.arm_selection = 1 else: options.arm_selection = 0 self.publish_feedback('Initial pickup failed, trying other arm') self.reset_collision_map() self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: return None else: # now check if feedback to see if we actually got it if options.arm_selection is 0: arm = 'right' else: arm = 'left' self.publish_feedback('Checking if object was grasped') resp = self.grasp_check(arm) if resp.isGrasping is True: self.publish_feedback('Object was grasped') # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) res = self.backup.get_result() # if robot could not back up if not res.success: self.publish_feedback('Backup failed.') return None return options.arm_selection else: self.move_arm_to_side(options.arm_selection) return None # moves arm to sides def move_arm_to_side(self, arm_selection): goal = IMGUIGoal() goal.command.command = 4 goal.options.arm_selection = arm_selection goal.options.arm_action_choice = 0 goal.options.arm_planner_choice = 1 self.publish_feedback('Moving arm to the side using planner') self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try open loop self.publish_feedback('Planned arm move failed, trying open loop') goal.options.arm_planner_choice = 0 self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: self.publish_feedback('Arm move failed.') return False else: self.publish_feedback('Arm move was successful') return True # place object in given arm to given pose def place_object(self, obj_info_orig, arm_selection, pose): #drive to the table self.publish_feedback('Driving robot to table') nav_goal = navigateGoal('Dining Table Nav', True) self.play('/home/rctoris/wav/run.wav') self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Table alignment failed.') return False self.publish_feedback('Aligned robot to table') self.reset_collision_map() self.publish_feedback('Attempting to place the object') # make a copy obj_info = copy.deepcopy(obj_info_orig) obj = obj_info.obj goal = PlaceGoal() # set the arm if arm_selection is 0: goal.arm_name = 'right_arm' prefix = 'r' else: goal.arm_name = 'left_arm' prefix = 'l' # rotate and "gu-chunk" orig_z = pose.position.z pose.orientation.x = 0 pose.orientation.y = 0 goal.place_locations = [] #iterating through possible x-locations to place object for x in range(0, 10): pose.position.x = pose.position.x + ((x - 5) * 0.0025) #iterating through possible y-locations to place object for y in range(0, 10): pose.position.y = pose.position.y + ((y - 5) * 0.0025) # 'i' is for some rotations for i in range(0, 10): rads = (pi * (i/10.0)) pose.orientation.z = sin(-rads/2.0) pose.orientation.w = cos(-rads/2.0); # 'j' is for the 'z' height for j in range (0, 6): pose.position.z = orig_z + (j * 0.0025) pose_mat = pose_to_mat(pose) to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose) gripper_mat = to_base_link_mat * grasp_mat gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link') goal.place_locations.append(gripper_pose) # use the identity as the grasp obj_info.grasp.grasp_pose.pose = Pose() obj_info.grasp.grasp_pose.pose.orientation.w = 1 goal.grasp = obj_info.grasp goal.desired_retreat_distance = 0.1 goal.min_retreat_distance = 0.05 # set the approach goal.approach = GripperTranslation() goal.approach.desired_distance = .1 goal.approach.min_distance = 0.05 goal.approach.direction = create_vector3_stamped([0. , 0. , -1.], 'base_link') # set the collision info goal.collision_object_name = obj.collision_name goal.collision_support_surface_name = 'table' goal.place_padding = 0.0 goal.additional_link_padding = self.create_gripper_link_padding(prefix) goal.collision_support_surface_name = 'all' goal.allow_gripper_support_collision = True goal.use_reactive_place = False # send the goal self.place.send_goal(goal) # wait for result finished_within_time = self.place.wait_for_result(rospy.Duration(240)) if not finished_within_time: self.place.cancel_goal() return False # check the result res = self.place.get_result() if res.manipulation_result.value == -6 or res.manipulation_result.value == 1: # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) backup_res = self.backup.get_result() # if robot could not back up if not backup_res.success: self.publish_feedback('Backup failed.') return False self.move_arm_to_side(arm_selection) return True else: return False def create_gripper_link_padding(self, prefix): link_name_list = ['_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link', '_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link'] pad = 0. arm_link_names = [prefix + link_name for link_name in link_name_list] link_padding_list = [LinkPadding(link_name, pad) for link_name in arm_link_names] return link_padding_list def create_object_info(self, obj): # get the pose pose_stamped = obj.potential_models[0].pose # change the frame obj_frame_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, obj.reference_frame_id) return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class PR2TrajectoryMarkers(object): """ A class to create and store a trajectory for one PR2 arm. The created trajectory can be published as a PoseArray message. This class published on the following topics: ~trajectory_markers are the main interactive markers. ~trajectory_poses a markerarray to display the trajectory. ~trajectory_poses a posesarray with the resulting pose The class subscribes to the topic ~overwrite_trajectory_ to change the stored trajectory. This is useful to resume working on a trajectory after re-starting the node. The message type is PoseArray. A std_srvs/Empty service named ~execute_trajectory is provided to externally trigger the execution of the trajectory. Constructor: TrajectoryMarkers(whicharm = "left") or TrajectoryMarkers(whicharm = "right") """ def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) self.gripper_pose_pub = rospy.Publisher("~gripper_pose", PoseStamped) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker); #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik self.planning_waiting_time = 10.0 rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm) 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) def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ #publish the gripper pose gripper_pos = PoseStamped() gripper_pos.header.frame_id = feedback.header.frame_id gripper_pos.pose = feedback.pose self.gripper_pose_pub.publish(gripper_pos) if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose if (self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP): self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1,0,0,1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges() def overwrite_trajectory(self, msg): self.trajectory = msg def add_point(self, feedback): """ Add a point to self.trajectory if it is allowed by IK. """ pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): rospy.loginfo("Pose is reachable") self.trajectory.poses.append(feedback.pose) else: rospy.logerr("Pose is not reachable!") def place_gripper(self, feedback): """ Move the marker where the gripper is """ if self.whicharm == "right": gripper_pos = self.planner.get_right_gripper_pose() else: gripper_pos = self.planner.get_left_gripper_pose() self.server.setPose(self.int_marker.name, gripper_pos.pose, gripper_pos.header) self.server.applyChanges() def execute_trajectory(self, feedback): """ Executes the tracjectory memorized so far. It interpolates between the poses and creates suitable times and velocities. """ traj = self.interpolate_poses() if len(traj) == 0: rospy.logerr("Something went wrong when interpolating") return times, vels = self.ik_utils.trajectory_times_and_vels(traj) if len(vels) == 0 or len(times) == 0: rospy.logerr("Something went wrong when finding the times") return self.joint_controller.execute_trajectory(traj, times, vels, self.whicharm, wait = True) return def execute_trajectory_srv(self, _): """Same as execute_trajectory, but as a service. """ self.execute_trajectory(None) return EmptyResponse() def arm_trajectory_start(self, feedback): """ Move the gripper to the first pose in the trajectory. """ if len(self.trajectory.poses) == 0: rospy.logwarn("Empty trajectory!") return pose = self.trajectory.poses[0] if self.whicharm == "right": moveit = self.planner.move_right_arm_non_collision else: moveit = self.planner.move_left_arm_non_collision pos, quat = create_tuples_from_pose(pose) res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0) if not res: rospy.logerr("Something went wrong when moving") return def clear_trajectory(self, feedback): """ Removes all the points stored so far """ self.trajectory.poses = [] def move_head(self, feedback): """ Moves the head to face the marker """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) print "Moving the head" self.joint_controller.time_to_reach = 1.0 self.joint_controller.point_head_to(pos, frame) def plan_arm(self, feedback): """ Moves the arm on the marker using motion collision-aware motion planning. """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = (feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm") self.planner.move_right_arm(pos, orientation, frame, self.planning_waiting_time) else: rospy.loginfo("Moving the left arm") self.planner.move_left_arm(pos, orientation, frame, self.planning_waiting_time) def collision_free_arm(self, feedback): """ Moves the rm on the marker using motion NON-collision-aware inverse kinematiks. """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = (feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm (non collision)") self.planner.move_right_arm_non_collision(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm (non collision)") self.planner.move_left_arm_non_collision(pos, orientation, frame, 2.0) def update_planning(self, feedback): """ Updates the planning scene. """ self.planner.take_static_map() self.planner.update_planning_scene() def publish_trajectory(self, feedback): """ Publishes the trajectory as a PoseArray message """ self.trajectory_pub.publish(self.trajectory) def interpolate_poses(self): """ Refines the trajectory by interpolating between the joints. """ if len(self.trajectory.poses) < 2: rospy.logerr("At least two points in the trajectory are needed") return [] if self.whicharm == "right": starting_angles = self.robot_state.right_arm_positions else: starting_angles = self.robot_state.left_arm_positions all_trajectory = [starting_angles] for i in xrange(len(self.trajectory.poses) - 1): start = PoseStamped() start.header = self.trajectory.header start.pose = self.trajectory.poses[i] end = PoseStamped() end.header = self.trajectory.header end.pose = self.trajectory.poses[i+1] traj, errs = self.ik_utils.check_cartesian_path(start, end, all_trajectory[-1], #starting_angles, #pos_spacing = 0.05, collision_aware = 0, num_steps = 5, ) if any(e == 3 for e in errs): rospy.logerr("Error while running IK, codes are: %s", errs) return [] filtered_traj = [t for (t,e) in zip(traj,errs) if e == 0] all_trajectory.extend(filtered_traj) all_trajectory = normalize_trajectory(all_trajectory, starting_angles) rospy.loginfo("New interpolated trajectory with %d elements", len(all_trajectory)) return all_trajectory def publish_trajectory_markers(self, duration): """ Publishes markers to visualize the current trajectory. Paremeters: duration: how long should the markers visualization last. If this function is called from a loop they last at least the loop rate. """ if len(self.trajectory.poses) == 0: return msg = MarkerArray() marker_id = 0 #creating the path connecting the axes path = Marker() path.header.frame_id = self.trajectory.header.frame_id path.ns = "path" path.action = Marker.ADD path.type = Marker.LINE_STRIP path.lifetime = rospy.Duration(duration) path.color.r = 1 path.color.g = 0 path.color.b = 1 path.color.a = 1 path.scale.x = 0.01 path.id = marker_id marker_id += 1 for pose in self.trajectory.poses: pos = PoseStamped() pos.header.frame_id = self.trajectory.header.frame_id pos.pose = pose markers = utils.axis_marker(pos, marker_id, "axes") msg.markers.extend(markers) path.points.append(pose.position) marker_id += 3 #3 axes msg.markers.append(path) self.visualizer_pub.publish(msg)
class POIServer(object): ''' Node to act as a server to hold a list of points of interest which can be modified by services or interactive markers ''' def __init__(self): ''' Create a POIServer ''' # TF bootstrap self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # Radius of interactive marker for POIs self.marker_scale = rospy.get_param("~marker_scale", 0.5) # Create intial empty POI array self.pois = POIArray() # Get the global frame to be used self.global_frame = rospy.get_param("~global_frame", "map") self.pois.header.frame_id = self.global_frame # Create publisher to notify clients of updates and interactive marker server self.pub = rospy.Publisher("points_of_interest", POIArray, queue_size=1, latch=True) self.interactive_marker_server = InteractiveMarkerServer( "points_of_interest") # Load initial POIs from params if rospy.has_param('~initial_pois'): pois = rospy.get_param('~initial_pois') assert isinstance(pois, dict) for key, value in pois.iteritems(): assert type(key) == str assert type(value) == list assert len(value) == 3 name = key position = numpy_to_point(value) self._add_poi(name, position) # Update clients / markers of changes from param self.update() # Create services to add / delete / move a POI self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb) self.delete_poi_server = rospy.Service('~delete', DeletePOI, self.delete_poi_cb) self.move_poi_service = rospy.Service('~move', MovePOI, self.move_poi_cb) self.save_to_param = rospy.Service("~save_to_param", Trigger, self.save_to_param_cb) def transform_position(self, ps): ''' Attempty to transform a PointStamped message into the global frame, returning the position of the transformed point or None if transform failed. ''' # If no frame, assume user wanted it in the global frame if ps.header.frame_id == "": return ps.point try: ps_tf = self.tf_buffer.transform(ps, self.global_frame, timeout=rospy.Duration(5)) return ps_tf.point except tf2_ros.TransformException as e: rospy.logwarn('Error transforming "{}" to "{}": {}'.format( ps.header.frame_id, self.global_frame, e)) return None def process_feedback(self, feedback): ''' Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ ''' # Only look at changes when mouse button is unclicked if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP: return # Transform new position into global frame ps = PointStamped() ps.header = feedback.header ps.point = feedback.pose.position position = self.transform_position(ps) if position is None: return # Update position of marker self.update_position(feedback.marker_name, feedback.pose.position) @thread_lock(lock) def save_to_param_cb(self, req): rospy.set_param('~global_frame', self.global_frame) d = {} for poi in self.pois.pois: d[poi.name] = [ float(poi.position.x), float(poi.position.y), float(poi.position.z) ] rospy.set_param('~initial_pois', d) return {'success': True} def add_poi_cb(self, req): ''' Callback for AddPOI service ''' name = req.name # If frame is empty, just initialize it to zero if len(req.position.header.frame_id) == 0: position = numpy_to_point([0., 0., 0.]) # Otherwise transform position into global frame else: position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} if not self.add_poi(name, position): return {'success': False, 'message': 'alread exists (bad poi)'} return {'success': True, 'message': 'good poi'} def delete_poi_cb(self, req): ''' Callback for DeletePOI service ''' # Return error if marker did not exist if not self.remove_poi(req.name): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} def move_poi_cb(self, req): ''' Callback for MovePOI service ''' name = req.name # Transform position into global frame position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} # Return error if marker did not exist if not self.update_position(name, position): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} @thread_lock(lock) def add_poi(self, name, position): ''' Add a POI, updating clients / rviz when done @return False if POI already exists ''' if not self._add_poi(name, position): return False self.update() return True @thread_lock(lock) def remove_poi(self, name): ''' Remove a POI, updating clients / rviz when done @return False if POI with name does not exist ''' if not self._remove_poi(name): return False self.update() return True @thread_lock(lock) def update_position(self, name, position): ''' Update the position of a POI, updating clients / rviz when done @param position: a Point message of the new position in global frame @return False if POI with name does not exist ''' if not self._update_position(name, position): return False self.update() return True def update(self): ''' Update interactive markers server and POI publish of changes ''' self.pois.header.stamp = rospy.Time.now() self.pub.publish(self.pois) self.interactive_marker_server.applyChanges() def _update_position(self, name, position): ''' Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change ''' # Find poi with specified name for poi in self.pois.pois: if poi.name == name: pose = Pose() pose.orientation.w = 1.0 pose.position = position if not self.interactive_marker_server.setPose(name, pose): return False # Set pose in message poi.position = position return True return False def _remove_poi(self, name): ''' Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change ''' # Return false if marker with that name not added to interactive marker server if not self.interactive_marker_server.erase(name): return False # Find POI with specifed name and delete it from list for i, poi in enumerate(self.pois.pois): if poi.name == name: del self.pois.pois[i] return True return False def _add_poi(self, name, position): ''' Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change ''' if self.interactive_marker_server.get(name) is not None: return False poi = POI() poi.name = name poi.position = position self.pois.pois.append(poi) point_marker = Marker() point_marker.type = Marker.SPHERE point_marker.scale.x = self.marker_scale point_marker.scale.y = self.marker_scale point_marker.scale.z = self.marker_scale point_marker.color.r = 1.0 point_marker.color.g = 1.0 point_marker.color.b = 1.0 point_marker.color.a = 1.0 text_marker = Marker() text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.orientation.w = 1.0 text_marker.pose.position.x = 1.5 text_marker.text = poi.name text_marker.scale.z = 1.0 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 text_marker.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = self.global_frame int_marker.pose.orientation.w = 1.0 int_marker.pose.position = poi.position int_marker.scale = 1 int_marker.name = poi.name # insert a box control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.always_visible = True control.markers.append(point_marker) control.markers.append(text_marker) int_marker.controls.append(control) self.interactive_marker_server.insert(int_marker, self.process_feedback) return True
class InteractiveMarkerManager(object): 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 _process_feedback(self, feedback): 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 + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") self._process_menu_select(feedback) elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback rospy.logdebug(s + ": pose changed") self._update_waypoint_position(feedback.marker_name, feedback.pose) elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") pass self.server.applyChanges() def _process_menu_select(self, feedback): menu_entry_id = feedback.menu_entry_id if menu_entry_id == self._menu_cmds['del_wp']: self._delete_waypoint(feedback) elif menu_entry_id == self._menu_cmds['ins_wp_before']: self._insert_waypoint(feedback, before=True) elif menu_entry_id == self._menu_cmds['ins_wp_after']: self._insert_waypoint(feedback, before=False) elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']: self._process_menu_time_cmds(feedback) else: pass def _delete_waypoint(self, feedback): marker = self.name_to_marker_dict[feedback.marker_name] waypoint = self.name_to_waypoint_dict[marker.name] self.parent_instance_handle.delete_waypoint(waypoint) # Update dictionaries del self.name_to_marker_dict[marker.name] del self.waypoint_to_marker_dict[waypoint] del self.name_to_waypoint_dict[marker.name] del self._menu_handlers[marker.name] del self._menu_cmds[marker.name] # Erase marker from server self.server.erase(feedback.marker_name) self.server.applyChanges() self.marker_cnt -=1 self._int_marker_name_list.remove(marker.name) self._update_wp_marker_descriptions() self.server.applyChanges() def _insert_waypoint(self, feedback, before=True): ref_marker = self.name_to_marker_dict[feedback.marker_name] ref_waypoint = self.name_to_waypoint_dict[ref_marker.name] self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before) self._update_wp_marker_descriptions() self.server.applyChanges() def _process_menu_time_cmds(self, feedback): menu_entry_handle = feedback.menu_entry_id menu_handler = self._menu_handlers[feedback.marker_name] check_state = menu_handler.getCheckState(menu_entry_handle) if check_state == MenuHandler.UNCHECKED: menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED) # Uncheck all other menu entries for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']: if menu_entry_id != menu_entry_handle: menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED) # Update waypoints waypoint = self.name_to_waypoint_dict[feedback.marker_name] waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle] # Apply marker changes menu_handler.reApply(self.server) self.server.applyChanges() def _update_waypoint_pose(self): # TODO maybe handle pose changes differently? pass def _update_waypoint_position(self, marker_name, marker_pose): waypoint = self.name_to_waypoint_dict[marker_name] eff_pos = marker_pose.position eff_orient = marker_pose.orientation target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions, eff_pos.x, eff_pos.y, eff_pos.z, eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z, self.ik_position_xyz_bounds[0], self.ik_position_xyz_bounds[1], self.ik_position_xyz_bounds[2], self.ik_position_rpy_bounds[0], self.ik_position_rpy_bounds[1], self.ik_position_rpy_bounds[2]) if target_jt_angles is not None: waypoint.end_effector_pose = copy.deepcopy(marker_pose) waypoint.joint_state.positions = target_jt_angles self._change_wp_marker_color(marker_name, "white", 0.9) else: self._change_wp_marker_color(marker_name, "red", 0.9) def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0): assert 0.0 <= opacity <= 1.0 # set color int_marker = self.server.get(wp_marker_name) marker = int_marker.controls[0].markers[0] replacement_int_marker = copy.deepcopy(int_marker) replacement_marker = replacement_int_marker.controls[0].markers[0] rgb = None if color == "white": rgb = (1.0, 1.0, 1.0) elif color == "red": rgb = (1.0, 0.0, 0.0) # TODO: Additional colors here changed = False if rgb is not None: if marker.color.r != rgb[0]: replacement_marker.color.r = rgb[0] changed = True print("change to ", rgb) if marker.color.g != rgb[1]: replacement_marker.color.g = rgb[1] changed = True if marker.color.b != rgb[2]: replacement_marker.color.b = rgb[2] changed = True if marker.color.a != opacity: replacement_marker.color.a = opacity changed = True # update wp marker if changed: self._replace_wp_marker(int_marker, replacement_int_marker) def _update_wp_marker_descriptions(self): for index, marker_name in enumerate(self._int_marker_name_list): marker = self.name_to_marker_dict[marker_name] marker_description = int(marker.description) if marker_description != index+1: marker.description = str(index+1) replacement_marker = copy.deepcopy(self.server.get(marker.name)) replacement_marker.description = marker.description self._replace_wp_marker(marker, replacement_marker) def _replace_wp_marker(self, old_int_marker, replacement_int_marker): # Update dictionaries self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker # Erase marker from server self.server.erase(old_int_marker.name) self.server.insert(replacement_int_marker, self._process_feedback) self.server.applyChanges() def add_waypoint_marker(self, waypoint, description=None): self.markers_created_cnt += 1 self.marker_cnt += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.pose.position = waypoint.end_effector_pose.position int_marker.scale = 0.1 int_marker.name = str(self.markers_created_cnt) if description is None: int_marker.description = int_marker.name else: int_marker.description = description self.name_to_marker_dict[int_marker.name] = int_marker self.waypoint_to_marker_dict[waypoint] = int_marker self.name_to_waypoint_dict[int_marker.name] = waypoint # insert a box # TODO: may change geometry / eff mesh make_box_control_marker(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 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 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 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 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 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 int_marker.controls.append(control) self.server.insert(int_marker, self._process_feedback) # add menus menu_handler = MenuHandler() del_sub_menu_handle = menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) # Set time time_sub_menu_handle = menu_handler.insert("Set Waypoint Time") menu_time_cmds = {} time = self.name_to_waypoint_dict[int_marker.name].time time_list = [float(num) for num in range((int(m.ceil(time))))] time_list.append(time) time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)]) self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds} for t in time_list: time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback) menu_time_cmds[time_opt_handle] = t if t == time: menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED) else: menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED) menu_handler.apply(self.server, int_marker.name) self._menu_handlers[int_marker.name] = menu_handler self.server.applyChanges() if int(int_marker.description)-1 < len(self._int_marker_name_list): self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name) else: self._int_marker_name_list.append(int_marker.name) def clear_waypoint_markers(self): self.server.clear() self.server.applyChanges() self._menu_handlers = {} self._menu_cmds = {} 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 = {}
class InteractiveMarkerManager(object): def __init__(self, server_ns='interactive_markers'): self.server = InteractiveMarkerServer(server_ns) self._menu_handlers = {} self._menu_cmds = {} self.markers_created_cnt = 0 self.marker_cnt = 0 self._int_marker_name_list = [] self.name_to_marker_dict = {} self.name_to_position_only_flag = {} def _process_feedback(self, feedback): 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 + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") self._process_menu_select(feedback) elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback rospy.logdebug(s + ": pose changed") self._update_marker_pose(feedback.marker_name, feedback.pose) elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") pass self.server.applyChanges() def _process_menu_select(self, feedback): menu_entry_id = feedback.menu_entry_id if menu_entry_id == self._menu_cmds['position_only']: self._set_position_only_checkmark(feedback) else: pass def _set_position_only_checkmark(self, feedback): menu_entry_handle = feedback.menu_entry_id menu_handler = self._menu_handlers[feedback.marker_name] check_state = menu_handler.getCheckState(menu_entry_handle) if check_state == MenuHandler.UNCHECKED: menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED) self.name_to_position_only_flag[feedback.marker_name] = True else: menu_handler.setCheckState(menu_entry_handle, MenuHandler.UNCHECKED) self.name_to_position_only_flag[feedback.marker_name] = False # Apply marker changes menu_handler.reApply(self.server) self.server.applyChanges() def change_marker_color(self, marker_name, color, opacity=1.0): assert 0.0 <= opacity <= 1.0 # set color int_marker = self.server.get(marker_name) marker = int_marker.controls[0].markers[0] replacement_int_marker = copy.deepcopy(int_marker) replacement_marker = replacement_int_marker.controls[0].markers[0] rgb = None if color == "white": rgb = (1.0, 1.0, 1.0) elif color == "red": rgb = (1.0, 0.0, 0.0) # TODO: Additional colors here changed = False if rgb is not None: if marker.color.r != rgb[0]: replacement_marker.color.r = rgb[0] changed = True print("change to ", rgb) if marker.color.g != rgb[1]: replacement_marker.color.g = rgb[1] changed = True if marker.color.b != rgb[2]: replacement_marker.color.b = rgb[2] changed = True if marker.color.a != opacity: replacement_marker.color.a = opacity changed = True # update marker if changed: self._replace_marker(int_marker, replacement_int_marker) def _replace_marker(self, old_int_marker, replacement_int_marker): # Update dictionaries self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker # Erase marker from server self.server.erase(old_int_marker.name) self.server.insert(replacement_int_marker, self._process_feedback) self.server.applyChanges() def add_marker(self, initial_pose, frame="base_link", description=None): self.markers_created_cnt += 1 self.marker_cnt += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.pose.position = initial_pose.position int_marker.scale = 0.1 int_marker.name = rospy.get_name() + str(self.markers_created_cnt) if description is None: int_marker.description = int_marker.name else: int_marker.description = description self.name_to_marker_dict[int_marker.name] = int_marker self.name_to_position_only_flag[int_marker.name] = True # insert a box # TODO: may change geometry / eff mesh make_box_control_marker(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 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 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 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 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 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 int_marker.controls.append(control) self.server.insert(int_marker, self._process_feedback) # add menus menu_handler = MenuHandler() pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback) self._menu_cmds['position_only'] = pos_opt_handle menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED) menu_handler.apply(self.server, int_marker.name) self._menu_handlers[int_marker.name] = menu_handler self.server.applyChanges() self._int_marker_name_list.append(int_marker.name) return int_marker.name def _update_marker_pose(self, marker_name, new_pose): self.name_to_marker_dict[marker_name].pose = new_pose def get_marker_pose(self, marker_name): return self.name_to_marker_dict[marker_name].pose def clear_markers(self): self.server.clear() self.server.applyChanges() self._menu_handlers = {} self._menu_cmds = {} self.markers_created_cnt = 0 self.marker_cnt = 0 self._int_marker_name_list = [] self.name_to_marker_dict = {} self.name_to_position_only_flag = {}
class PR2TrajectoryMarkers(object): """ A class to create and store a trajectory for one PR2 arm. The created trajectory can be published as a PoseArray message. This class published on the following topics: ~trajectory_markers are the main interactive markers. ~trajectory_poses a markerarray to display the trajectory. ~trajectory_poses a posesarray with the resulting pose The class subscribes to the topic ~overwrite_trajectory_ to change the stored trajectory. This is useful to resume working on a trajectory after re-starting the node. The message type is PoseArray. A std_srvs/Empty service named ~execute_trajectory is provided to externally trigger the execution of the trajectory. Constructor: TrajectoryMarkers(whicharm = "left") or TrajectoryMarkers(whicharm = "right") """ def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover( self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm) 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) def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose #rospy.loginfo("Updating Marker: %d", feedback.event_type) #rospy.loginfo("POS: %s", feedback.pose.position) if (self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP): self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1, 0, 0, 1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges() def overwrite_trajectory(self, msg): self.trajectory = msg def add_point(self, feedback): """ Add a point to self.trajectory if it is allowed by IK. """ pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): rospy.loginfo("Pose is reachable") self.trajectory.poses.append(feedback.pose) else: rospy.logerr("Pose is not reachable!") def place_gripper(self, feedback): """ Move the marker where the gripper is """ if self.whicharm == "right": gripper_pos = self.planner.get_right_gripper_pose() else: gripper_pos = self.planner.get_left_gripper_pose() self.server.setPose(self.int_marker.name, gripper_pos.pose, gripper_pos.header) self.server.applyChanges() def execute_trajectory(self, feedback): """ Executes the tracjectory memorized so far. It interpolates between the poses and creates suitable times and velocities. """ traj = self.interpolate_poses() if len(traj) == 0: rospy.logerr("Something went wrong when interpolating") return times, vels = self.ik_utils.trajectory_times_and_vels(traj) if len(vels) == 0 or len(times) == 0: rospy.logerr("Something went wrong when finding the times") return self.joint_controller.execute_trajectory(traj, times, vels, self.whicharm, wait=True) return def execute_trajectory_srv(self, _): """Same as execute_trajectory, but as a service. """ self.execute_trajectory(None) return EmptyResponse() def arm_trajectory_start(self, feedback): """ Move the gripper to the first pose in the trajectory. """ if len(self.trajectory.poses) == 0: rospy.logwarn("Empty trajectory!") return pose = self.trajectory.poses[0] if self.whicharm == "right": moveit = self.planner.move_right_arm_non_collision else: moveit = self.planner.move_left_arm_non_collision pos, quat = create_tuples_from_pose(pose) res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0) if not res: rospy.logerr("Something went wrong when moving") return def clear_trajectory(self, feedback): """ Removes all the points stored so far """ self.trajectory.poses = [] def move_head(self, feedback): """ Moves the head to face the marker """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) print "Moving the head" self.joint_controller.time_to_reach = 1.0 self.joint_controller.point_head_to(pos, frame) def plan_arm(self, feedback): """ Moves the arm on the marker using motion collision-aware motion planning. """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = ( feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm") self.planner.move_right_arm(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm") self.planner.move_left_arm(pos, orientation, frame, 2.0) def collision_free_arm(self, feedback): """ Moves the rm on the marker using motion NON-collision-aware inverse kinematiks. """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = ( feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm (non collision)") self.planner.move_right_arm_non_collision(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm (non collision)") self.planner.move_left_arm_non_collision(pos, orientation, frame, 2.0) def update_planning(self, feedback): """ Updates the planning scene. """ self.planner.take_static_map() self.planner.update_planning_scene() def publish_trajectory(self, feedback): """ Publishes the trajectory as a PoseArray message """ self.trajectory_pub.publish(self.trajectory) def interpolate_poses(self): """ Refines the trajectory by interpolating between the joints. """ if len(self.trajectory.poses) < 2: rospy.logerr("At least two points in the trajectory are needed") return [] if self.whicharm == "right": starting_angles = self.robot_state.right_arm_positions else: starting_angles = self.robot_state.left_arm_positions all_trajectory = [starting_angles] for i in xrange(len(self.trajectory.poses) - 1): start = PoseStamped() start.header = self.trajectory.header start.pose = self.trajectory.poses[i] end = PoseStamped() end.header = self.trajectory.header end.pose = self.trajectory.poses[i + 1] traj, errs = self.ik_utils.check_cartesian_path( start, end, all_trajectory[-1], #starting_angles, #pos_spacing = 0.05, collision_aware=0, num_steps=5, ) if any(e == 3 for e in errs): rospy.logerr("Error while running IK, codes are: %s", errs) return [] filtered_traj = [t for (t, e) in zip(traj, errs) if e == 0] all_trajectory.extend(filtered_traj) all_trajectory = normalize_trajectory(all_trajectory, starting_angles) rospy.loginfo("New interpolated a trajectory with %d elements", len(all_trajectory)) return all_trajectory def publish_trajectory_markers(self, duration): """ Publishes markers to visualize the current trajectory. Paremeters: duration: how long should the markers visualization last. If this function is called from a loop they last at least the loop rate. """ if len(self.trajectory.poses) == 0: return msg = MarkerArray() marker_id = 0 #creating the path connecting the axes path = Marker() path.header.frame_id = self.trajectory.header.frame_id path.ns = "path" path.action = Marker.ADD path.type = Marker.LINE_STRIP path.lifetime = rospy.Duration(duration) path.color.r = 1 path.color.g = 0 path.color.b = 1 path.color.a = 1 path.scale.x = 0.01 path.id = marker_id marker_id += 1 for pose in self.trajectory.poses: pos = PoseStamped() pos.header.frame_id = self.trajectory.header.frame_id pos.pose = pose markers = utils.axis_marker(pos, marker_id, "axes") msg.markers.extend(markers) path.points.append(pose.position) marker_id += 3 #3 axes msg.markers.append(path) self.visualizer_pub.publish(msg)
class IntCollisionEnv(CollisionEnvServices): def __init__(self, setup, world_frame): super(IntCollisionEnv, self).__init__(setup, world_frame) self.im_server = InteractiveMarkerServer(self.NS + "markers") self.check_attached_timer = rospy.Timer(rospy.Duration(0.1), self.check_attached_timer_cb) self.create_menus() def create_menus(self): self.global_menu_handler = MenuHandler() g_art = self.global_menu_handler.insert("Artificial") self.global_menu_handler.insert("Add primitive", parent=g_art, callback=self.global_menu_add_prim_cb) self.global_menu_handler.insert("Clear all", parent=g_art, callback=self.global_menu_clear_all_cb) self.global_menu_handler.insert( "Clear all (permanent)", parent=g_art, callback=self.global_menu_clear_all_perm_cb) self.global_menu_handler.insert("Save all", parent=g_art, callback=self.global_menu_save_all_cb) self.global_menu_handler.insert("Reload", parent=g_art, callback=self.global_menu_reload_cb) g_det = self.global_menu_handler.insert("Detected") self.global_menu_handler.insert("Pause", parent=g_det, callback=self.global_menu_pause_cb) self.global_menu_handler.insert( "Clear all", parent=g_det, callback=self.global_menu_det_clear_all_cb) self.a_obj_menu_handler = MenuHandler() mov = self.a_obj_menu_handler.insert("Moveable", callback=self.menu_moveable_cb) self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED) sc = self.a_obj_menu_handler.insert("Scaleable", callback=self.menu_scaleable_cb) self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED) self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb) self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb) self.d_obj_menu_handler = MenuHandler() self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb) int_marker = InteractiveMarker() int_marker.header.frame_id = self.world_frame int_marker.pose.position.z = 1.2 int_marker.scale = 0.5 int_marker.name = "global_menu" int_marker.description = "Global Menu" control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.BUTTON control.always_visible = True marker = Marker() marker.type = Marker.CUBE marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 control.markers.append(marker) int_marker.controls.append(control) self.im_server.insert(int_marker, self.ignored_cb) self.global_menu_handler.apply(self.im_server, int_marker.name) self.im_server.applyChanges() def ignored_cb(self, feedback): pass def d_menu_clear_cb(self, f): pass def global_menu_det_clear_all_cb(self, f): for name in self.clear_all_det(): self.im_server.erase(name) self.im_server.applyChanges() def global_menu_pause_cb(self, f): handle = f.menu_entry_id state = self.global_menu_handler.getCheckState(handle) if state == MenuHandler.CHECKED: self.global_menu_handler.setCheckState(handle, MenuHandler.UNCHECKED) self.paused = False else: self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED) self.paused = True self.global_menu_handler.reApply(self.im_server) self.im_server.applyChanges() def global_menu_save_all_cb(self, f): self.save_primitives() def global_menu_add_prim_cb(self, feedback): p = CollisionPrimitive() p.name = self._generate_name() p.bbox.type = SolidPrimitive.BOX p.bbox.dimensions = [0.1, 0.1, 0.1] p.pose.header.frame_id = self.world_frame p.pose.pose.orientation.w = 1 p.pose.pose.position.z = 0.5 p.setup = self.setup self.add_primitive(p) def global_menu_reload_cb(self, feedback): self.reload() def global_menu_clear_all_cb(self, feedback): self.clear_all(permanent=False) def global_menu_clear_all_perm_cb(self, feedback): self.clear_all() def menu_save_cb(self, f): self.save_primitive(f.marker_name) def menu_scaleable_cb(self, f): handle = f.menu_entry_id state = self.a_obj_menu_handler.getCheckState(handle) if state == MenuHandler.CHECKED: self.a_obj_menu_handler.setCheckState(handle, MenuHandler.UNCHECKED) # TODO else: self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED) # TODO self.a_obj_menu_handler.reApply(self.im_server) self.im_server.applyChanges() def menu_moveable_cb(self, feedback): handle = feedback.menu_entry_id state = self.a_obj_menu_handler.getCheckState(handle) if state == MenuHandler.CHECKED: self.a_obj_menu_handler.setCheckState(handle, MenuHandler.UNCHECKED) # if feedback.marker_name in self.moveable: # self.moveable.remove(feedback.marker_name) self.im_server.erase(feedback.marker_name) self.im_server.insert( make_def(self.artificial_objects[feedback.marker_name]), self.process_im_feedback) else: self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED) self.make_interactive( self.artificial_objects[feedback.marker_name]) self.a_obj_menu_handler.reApply(self.im_server) self.im_server.applyChanges() def menu_remove_cb(self, feedback): self.remove_name(feedback.marker_name) def process_im_feedback(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: ps = PoseStamped() ps.header = feedback.header ps.pose = feedback.pose if feedback.marker_name in self.artificial_objects: self.set_primitive_pose(feedback.marker_name, ps) elif feedback.marker_name: # detected objects pass def make_interactive(self, p): im = self.im_server.get(p.name) for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0, 1))): im.controls.append(rotate_axis_control("rotate_" + v[0], v[1])) im.controls.append(move_axis_control("move_" + v[0], v[1])) def remove_name(self, name): super(IntCollisionEnv, self).remove_name(name) self.im_server.erase(name) self.im_server.applyChanges() def add_primitive(self, p): super(IntCollisionEnv, self).add_primitive(p) self.im_server.insert(make_def(p), self.process_im_feedback) self.a_obj_menu_handler.apply(self.im_server, p.name) self.im_server.applyChanges() def reload(self): self.im_server.clear() self.create_menus() super(IntCollisionEnv, self).reload() def add_detected(self, name, ps, object_type): super(IntCollisionEnv, self).add_detected(name, ps, object_type) p = CollisionPrimitive() p.name = name p.pose = ps p.bbox = object_type.bbox self.im_server.insert(make_def(p, color=(0, 0, 1)), self.process_im_feedback) self.im_server.applyChanges() def clear_detected(self, name): super(IntCollisionEnv, self).clear_detected(name) self.im_server.erase(name) self.im_server.applyChanges() def check_attached_timer_cb(self, evt): for name, ps, bbox in self.get_attached(): p = CollisionPrimitive() p.name = name p.pose = ps p.bbox = bbox self.im_server.insert(make_def(p, color=(1, 0, 0)), self.process_im_feedback) self.im_server.applyChanges()