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 PickAndPlaceNode(Manager): def __init__(self, robot, *robotargs): super(PickAndPlaceNode, self).__init__('pp_node') rospy.loginfo("PickAndPlaceNode") _post_perceive_trans = defaultdict(lambda: self._pick) _post_perceive_trans.update({'c': self._calibrate}) _preplace = defaultdict(lambda: self._preplace) self.transition_table = { # If calibration has already happened, allow skipping it 'initial': {'c': self._calibrate, 'q': self._perceive, 's': self._preplace}, 'calibrate': {'q': self._perceive, 'c': self._calibrate}, 'perceive': {'p': self._post_perceive, 'q': self._perceive, 's': self._stop_perceive, 'c': self._calibrate}, 'post_perceive': _post_perceive_trans, 'postpick': {'1': self._level, '2': self._level, '9': self._level}, 'level': _preplace, 'preplace': {'s': self._place}, 'place': {'q': self._perceive, 'c': self._calibrate} } rospy.loginfo("PickAndPlaceNode1") if callable(robot): self.robot = robot(*robotargs) else: self.robot = robot self.robot.level = 1 rospy.loginfo("PickAndPlaceNode2") # Hardcoded place for now self.place_pose = PoseStamped( Header(0, rospy.Time(0), self.robot.base), Pose(Point(0.526025806, 0.4780144, -0.161326153), Quaternion(1, 0, 0, 0))) self.tl = tf.TransformListener() self.num_objects = 0 # Would this work too? Both tf and tf2 have (c) 2008... # self.tf2 = tf2_ros.TransformListener() self.n_objects_sub = rospy.Subscriber("/num_objects", Int64, self.update_num_objects, queue_size=1) self.perception_pub = rospy.Publisher("/perception/enabled", Bool, queue_size=1) self.alignment_pub = rospy.Publisher("/alignment/doit", Bool, queue_size=1) self.br = tf.TransformBroadcaster() rospy.loginfo("PickAndPlaceNode3") self.int_marker_server = InteractiveMarkerServer('int_markers') # Dict to map imarker names and their updated poses self.int_markers = {} # Ideally this call would be in a Factory/Metaclass/Parent self.show_options() self.perceive = False # self.robot.home() # self.move_calib_position() def move_calib_position(self): # move arm to the calibration position self.calib_pose = PoseStamped( Header(0, rospy.Time(0), self.robot.base), Pose(Point(0.338520675898,-0.175860479474,0.0356775075197), Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013))) self.robot.move_ik(self.calib_pose) def update_num_objects(self, msg): self.num_objects = msg.data def _calibrate(self): self.state = "calibrate" self.alignment_pub.publish(Bool(True)) def _perceive(self): self.state = "perceive" rospy.loginfo("Asking for perception...") self.perception_pub.publish(Bool(True)) def _stop_perceive(self): self.state = "perceive" self.perception_pub.publish(Bool(False)) def _post_perceive(self): self.state = "post_perceive" rospy.loginfo("Asking to stop perception...") self.perception_pub.publish(Bool(False)) def _preplace(self): # State not modified until placing succeeds try: obj_to_get = int(self.character) except ValueError: rospy.logerr("Please provide a number in placing mode") return self.state = "preplace" frame_name = "object_pose_{}".format(obj_to_get) rospy.loginfo( "Placing object on top of object {}...".format(obj_to_get)) if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name): t = self.tl.getLatestCommonTime(self.robot.base, frame_name) position, quaternion = self.tl.lookupTransform(self.robot.base, frame_name, t) position = list(position) # Height of cubelet position[2] += self.robot.level * 0.042 # YCB # position[2] += self.robot.level * 0.026 # Update pose position from perception self.place_pose.pose.position = Point(*position) rospy.loginfo("Adjusting place position...") imarker_name = 'place_pose' self.int_markers[imarker_name] = self.place_pose imarker = make_interactive_marker(imarker_name, self.place_pose.pose) # TODO delete imarker at post place self.int_marker_server.insert(imarker, self.imarker_callback) self.int_marker_server.setCallback(imarker_name, self.imarker_callback) self.int_marker_server.applyChanges() rospy.loginfo("imarker stuff done") def imarker_callback(self, msg): # http://docs.ros.org/jade/api/visualization_msgs/html/msg/InteractiveMarkerFeedback.html # noqa rospy.loginfo('imarker_callback called') name = msg.marker_name new_pose = msg.pose self.int_markers[name] = PoseStamped(msg.header, new_pose) def _place(self): self.state = "place" rospy.loginfo("Placing...") place_pose = self.int_markers['place_pose'] # It seems positions and orientations are randomly required to # be actual Point and Quaternion objects or lists/tuples. The # least coherent API ever seen. self.br.sendTransform(Point2list(place_pose.pose.position), Quaternion2list(place_pose.pose.orientation), rospy.Time.now(), "place_pose", self.robot.base) self.robot.place(place_pose) # For cubelets: place_pose.pose.position.z += 0.042 # For YCB: # place_pose.pose.position.z += 0.026 self.place_pose = place_pose def _pick(self): # State not modified until picking succeeds try: obj_to_get = int(self.character) except ValueError: rospy.logerr("Please provide a number in picking mode") return frame_name = "object_pose_{}".format(obj_to_get) rospy.loginfo("Picking object {}...".format(obj_to_get)) if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name): t = self.tl.getLatestCommonTime(self.robot.base, frame_name) position, quaternion = self.tl.lookupTransform(self.robot.base, frame_name, t) print("position", position) print("quaternion", quaternion) position = list(position) # Vertical orientation self.br.sendTransform(position, [1, 0, 0, 0], rospy.Time.now(), "pick_pose", self.robot.base) # Ignore orientation from perception pose = Pose(Point(*position), Quaternion(1, 0, 0, 0)) h = Header() h.stamp = t h.frame_id = self.robot.base stamped_pose = PoseStamped(h, pose) self.robot.pick(stamped_pose) self.state = 'postpick' def _level(self): self.robot.level = int(self.character) self.state = 'level'
class CircleMarker(object): def __init__(self): self._server = InteractiveMarkerServer("simple_marker") def callback_marker(self, input): pass def align_marker(self, feedback): pose = feedback.pose self._server.setPose(feedback.marker_name, pose) print("Arrow Marker's name is ", feedback.marker_name) self._server.setPose("orientation_ring", pose) self._server.applyChanges() def orientation_ring_callback(self, feedback): if (feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE): print("Updating arrow marker's position ") self._server.setPose("int_arrow_marker", feedback.pose) self._server.applyChanges() def create_draggable_marker(self, position): int_arrow_marker = InteractiveMarker() int_arrow_marker.header.frame_id = "map" int_arrow_marker.name = "int_arrow_marker" int_arrow_marker.description = "right" int_arrow_marker.pose.position.y = position.y int_arrow_marker.pose.position.x = position.x int_arrow_marker.pose.position.z = position.z + 0.1 int_arrow_marker.scale = 1 arrow_marker = Marker() arrow_marker.ns = "arrow_marker" arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 0 arrow_marker.scale.x = 0.5 arrow_marker.scale.y = 0.05 arrow_marker.scale.z = 0.05 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE button_control.always_visible = True button_control.orientation.w = 1 button_control.orientation.x = 0 button_control.orientation.y = 1 button_control.orientation.z = 0 button_control.markers.append(arrow_marker) int_arrow_marker.controls.append(button_control) orientation_ring_marker = InteractiveMarker() orientation_ring_marker.header.frame_id = "map" orientation_ring_marker.scale = 1 orientation_ring_marker.pose.position.y = position.y orientation_ring_marker.pose.position.z = position.z + 0.1 orientation_ring_marker.pose.position.x = position.x orientation_ring_marker.name = "orientation_ring" orientation_ring_marker.description = "Orientation Ring" orientation_ring_marker_control = InteractiveMarkerControl() orientation_ring_marker_control.always_visible = True orientation_ring_marker_control.orientation.w = 1 orientation_ring_marker_control.orientation.x = 0 orientation_ring_marker_control.orientation.y = 1 orientation_ring_marker_control.orientation.z = 0 orientation_ring_marker_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE orientation_ring_marker.controls.append(orientation_ring_marker_control) self._server.insert(orientation_ring_marker, self.orientation_ring_callback) self._server.insert(int_arrow_marker, self.callback_marker) print"inserted" self._server.setCallback("int_arrow_marker", self.align_marker, InteractiveMarkerFeedback.POSE_UPDATE) self._server.applyChanges()
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)