class RvizPathServer(object): def __init__(self): super(RvizPathServer, self).__init__() rospy.init_node("traversability_rviz_paths_node") self.server = InteractiveMarkerServer("paths") self.paths = {} self.delta_z = rospy.get_param('~offset', 0.15) self.width = rospy.get_param('~width', 0.15) self.pub = rospy.Publisher("selected_path", NavPath, queue_size=1) rospy.Subscriber("paths", Paths, self.updatePaths, queue_size=1) # self.add_marker(test_msg(), 0) # self.server.applyChanges() rospy.spin() def add_marker(self, msg, path_id): menu, marker = create_marker(path_msg=msg.path, color_msg=msg.color, description=msg.description, path_id=path_id, width=self.width, delta_z=self.delta_z) self.server.insert(marker, ignore) menu.insert("FOLLOW", callback=self.goto(path_id)) menu.apply(self.server, marker.name) self.paths[path_id] = msg.path def goto(self, path_id): def f(msg): rospy.loginfo("Follow path %d", path_id) self.pub.publish(self.paths[path_id]) return f def updatePaths(self, msg): path_msg = NavPath() path_msg.header.frame_id = 'map' self.pub.publish(path_msg) self.server.clear() for i, m in enumerate(msg.paths): self.add_marker(m, i) self.server.applyChanges()
class World: '''Object recognition and localization related stuff''' tf_listener = None objects = [] def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') bb_service_name = 'find_cluster_bounding_box' rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber('interactive_object_recognition_result', GraspableObjectList, self.receive_object_info) self._object_action_client = actionlib.SimpleActionClient( 'object_detection_user_command', UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo('Interactive object detection action ' + 'server has responded.') self.clear_all_objects() # The following is to get the table information rospy.Subscriber('tabletop_segmentation_markers', Marker, self.receive_table_marker) rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.receive_ar_markers) self.marker_dims = Vector3(0.04, 0.04, 0.01) def receive_ar_markers(self, data): '''Callback function to receive marker info''' self._lock.acquire() if len(data.markers) > 0: rospy.loginfo('Received non-empty marker list.') for i in range(len(data.markers)): marker = data.markers[i] self._add_new_object(marker.pose.pose, self.marker_dims, marker.id) self._lock.release() def _reset_objects(self): '''Function that removes all objects''' self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def get_tool_id(self): if (len(self.objects) == 0): rospy.warning('There are no fiducials, cannot get tool ID.') return None elif (len(self.objects) > 1): rospy.warning('There are more than one fiducials, returning the first as tool ID.') return World.objects[0].marker_id def get_surface(self): if (len(self.objects) < 4): rospy.warning('There are not enough fiducials to detect surface.') return None elif (len(self.objects) > 4): rospy.warning('There are more than four fiducials for surface, will use first four.') return points = [World.objects[0].position, World.objects[1].position, World.objects[2].position, World.objects[3].position] s = Surface(points) return s def receive_table_marker(self, marker): '''Callback function for markers to determine table''' if (marker.type == Marker.LINE_STRIP): if (len(marker.points) == 6): rospy.loginfo('Received a TABLE marker.') xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receive_object_info(self, object_list): '''Callback function to receive object info''' self._lock.acquire() rospy.loginfo('Received recognized object list.') if (len(object_list.graspable_objects) > 0): for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if (len(models) > 0): object_pose = None best_confidence = 0.0 for j in range(len(models)): if (best_confidence < models[j].confidence): object_pose = models[j].pose.pose best_confidence = models[j].confidence if (object_pose != None): rospy.logwarn('Adding the recognized object ' + 'with most confident model.') self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), i, object_list.meshes[i]) else: rospy.logwarn('... this is not a recognition result, ' + 'it is probably just segmentation.') cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if (cluster_pose != None): rospy.loginfo('Adding unrecognized object with pose:' + World.pose_to_string(cluster_pose) + '\n' + 'In ref frame' + str(bbox.pose.header.frame_id)) self._add_new_object(cluster_pose, bbox.box_dims, i) else: rospy.logwarn('... but the list was empty.') self._lock.release() @staticmethod def get_pose_from_transform(transform): '''Returns pose for transformation matrix''' pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) @staticmethod def get_matrix_from_pose(pose): '''Returns the transformation matrix for given pose''' rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation @staticmethod def get_absolute_pose(arm_state): '''Returns absolute pose of an end effector state''' if (arm_state.refFrame == ArmState.OBJECT): arm_state_copy = ArmState(arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def _get_mesh_marker(marker, mesh): '''Function that generated a marker from a mesh''' marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while (index + 2 < len(mesh.triangles)): if ((mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices))): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr('Mesh contains invalid triangle!') break return marker def _add_new_object(self, pose, dimensions, id=None, mesh=None): '''Function to add new objects''' dist_threshold = 0.02 to_remove = None for i in range(len(World.objects)): if (World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold): rospy.loginfo('Previously detected object at the same' + 'location, will not add this object.') return False n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, id)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _remove_object(self, to_remove): '''Function to remove object by index''' obj = World.objects.pop(to_remove) rospy.loginfo('Removing object ' + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() def _remove_surface(self): '''Function to request removing surface''' rospy.loginfo('Removing surface') self._im_server.erase('surface') self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker @staticmethod def get_frame_list(): '''Function that returns the list of ref. frames''' objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) return objects @staticmethod def has_objects(): '''Function that checks if there are any objects''' return len(World.objects) > 0 @staticmethod def get_ref_from_name(ref_name): '''Ref. frame type from ref. frame name''' if ref_name == 'base_link': return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): '''Transforms an arm frame to a new ref. frame''' if ref_frame == ArmState.ROBOT_BASE: if (arm_frame.refFrame == ArmState.ROBOT_BASE): pass #rospy.logwarn('No reference frame transformations ' + #'needed (both absolute).') elif (arm_frame.refFrame == ArmState.OBJECT): abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, 'base_link') arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) elif ref_frame == ArmState.OBJECT: if (arm_frame.refFrame == ArmState.ROBOT_BASE): rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link', ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif (arm_frame.refFrame == ArmState.OBJECT): if (arm_frame.refFrameObject.name == ref_frame_obj.name): pass #rospy.logwarn('No reference frame transformations ' + #'needed (same object).') else: rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) return arm_frame @staticmethod def has_object(object_name): '''Checks if the world contains the object''' for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): '''Checks if the frame is valid for transforms''' return (object_name == 'base_link') or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): ''' Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('Exception during transform.') return pose else: rospy.logwarn('One of the frame objects might not exist: ' + from_frame + ' or ' + to_frame) return pose @staticmethod def pose_to_string(pose): '''For printing a pose to stdout''' return ('Position: ' + str(pose.position.x) + ", " + str(pose.position.y) + ', ' + str(pose.position.z) + '\n' + 'Orientation: ' + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ', ' + str(pose.orientation.z) + ', ' + str(pose.orientation.w) + '\n') def _publish_tf_pose(self, pose, name, parent): ''' Publishes a TF for object pose''' if (pose != None): pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): ''' Function to externally update an object pose''' Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while (Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE): time.sleep(0.1) if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not look down to take table snapshot') return False rospy.loginfo('Looking at table now.') goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) self._reset_objects() if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not reset recognition.') return False # Do segmentation goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Table segmentation is complete.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not segment.') return False # Do recognition goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Objects on the table have been recognized.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) # Record the result if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): wait_time = 0 total_wait_time = 5 while (not World.has_objects() and wait_time < total_wait_time): time.sleep(0.1) wait_time += 0.1 if (not World.has_objects()): rospy.logerr('Timeout waiting for a recognition result.') return False else: rospy.loginfo('Got the object list.') return True else: rospy.logerr('Could not recognize.') return False def clear_all_objects(self): '''Removes all objects from the world''' goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('Successfully reset object localization pipeline.') self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): '''Gives a pointed to the nearest object''' dist_threshold = 0.4 def chObj(cur, obj): dist = World.pose_distance(obj.object.pose, arm_pose) return (dist, obj.object) if (dist < cur[0]) else cur return reduce(chObj, World.objects, (dist_threshold, None))[1] @staticmethod def pose_distance(pose1, pose2, is_on_table=True): '''Distance between two world poses''' if pose1 == [] or pose2 == []: return 0.0 else: if (is_on_table): arr1 = array([pose1.position.x, pose1.position.y]) arr2 = array([pose2.position.x, pose2.position.y]) else: arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z]) arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z]) dist = norm(arr1 - arr2) if dist < 0.0001: dist = 0 return dist def marker_feedback_cb(self, feedback): '''Callback for when feedback from a marker is received''' if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Clicked on object ' + str(feedback.marker_name)) rospy.loginfo('Number of objects ' + str(len(World.objects))) else: rospy.loginfo('Unknown event' + str(feedback.event_type)) def update(self): '''Update function called in a loop''' # Visualize the detected object is_world_changed = False self._lock.acquire() if (World.has_objects()): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), 'base_link') if (World.objects[i].is_removed): to_remove = i if to_remove != None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed
class World: """Object recognition and localization related stuff""" tf_listener = None objects = [] def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer("world_objects") bb_service_name = "find_cluster_bounding_box" rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info) self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo("Interactive object detection action " + "server has responded.") self.clear_all_objects() # The following is to get the table information rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker) def _reset_objects(self): """Function that removes all objects""" self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def receieve_table_marker(self, marker): """Callback function for markers to determine table""" if marker.type == Marker.LINE_STRIP: if len(marker.points) == 6: rospy.loginfo("Received a TABLE marker.") xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receieve_object_info(self, object_list): """Callback function to receive object info""" self._lock.acquire() rospy.loginfo("Received recognized object list.") if len(object_list.graspable_objects) > 0: for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if len(models) > 0: object_pose = None best_confidence = 0.0 for j in range(len(models)): if best_confidence < models[j].confidence: object_pose = models[j].pose.pose best_confidence = models[j].confidence if object_pose != None: rospy.logwarn("Adding the recognized object " + "with most confident model.") self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i]) else: rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.") cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if cluster_pose != None: rospy.loginfo( "Adding unrecognized object with pose:" + World.pose_to_string(cluster_pose) + "\n" + "In ref frame" + str(bbox.pose.header.frame_id) ) self._add_new_object(cluster_pose, bbox.box_dims, False) else: rospy.logwarn("... but the list was empty.") self._lock.release() @staticmethod def get_pose_from_transform(transform): """Returns pose for transformation matrix""" pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) @staticmethod def get_matrix_from_pose(pose): """Returns the transformation matrix for given pose""" rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation @staticmethod def get_absolute_pose(arm_state): """Returns absolute pose of an end effector state""" if arm_state.refFrame == ArmState.OBJECT: arm_state_copy = ArmState( arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject, ) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def get_most_similar_obj(ref_object, ref_frame_list): """Finds the most similar object in the world""" best_dist = 10000 chosen_obj_index = -1 for i in range(len(ref_frame_list)): dist = World.object_dissimilarity(ref_frame_list[i], ref_object) if dist < best_dist: best_dist = dist chosen_obj_index = i if chosen_obj_index == -1: rospy.logwarn("Did not find a similar object..") return None else: print "Object dissimilarity is --- ", best_dist if best_dist > 0.075: rospy.logwarn("Found some objects, but not similar enough.") return None else: rospy.loginfo("Most similar to new object " + str(chosen_obj_index)) return ref_frame_list[chosen_obj_index] @staticmethod def _get_mesh_marker(marker, mesh): """Function that generated a marker from a mesh""" marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while index + 2 < len(mesh.triangles): if ( (mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices)) ): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr("Mesh contains invalid triangle!") break return marker def _add_new_object(self, pose, dimensions, is_recognized, mesh=None): """Function to add new objects""" dist_threshold = 0.02 to_remove = None if is_recognized: # Temporary HACK for testing. # Will remove all recognition completely if this works. return False # Check if there is already an object for i in range(len(World.objects)): distance = World.pose_distance(World.objects[i].object.pose, pose) if distance < dist_threshold: if World.objects[i].is_recognized: rospy.loginfo( "Previously recognized object at " + "the same location, will not add this object." ) return False else: rospy.loginfo( "Previously unrecognized object at " + "the same location, will replace it with the " + "recognized object." ) to_remove = i break if to_remove != None: self._remove_object(to_remove) n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1, mesh) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True else: for i in range(len(World.objects)): if World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold: rospy.loginfo("Previously detected object at the same" + "location, will not add this object.") return False n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _remove_object(self, to_remove): """Function to remove object by index""" obj = World.objects.pop(to_remove) rospy.loginfo("Removing object " + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() # if (obj.is_recognized): # for i in range(len(World.objects)): # if ((World.objects[i].is_recognized) # and World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_recognized -= 1 # else: # for i in range(len(World.objects)): # if ((not World.objects[i].is_recognized) and # World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_unrecognized -= 1 def _remove_surface(self): """Function to request removing surface""" rospy.loginfo("Removing surface") self._im_server.erase("surface") self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): """Generate a marker for world objects""" int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = "base_link" int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id="base_link"), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose, ) if mesh != None: object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06 button_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id="base_link"), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)), ) ) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): """ Function that generates a surface marker""" int_marker = InteractiveMarker() int_marker.name = "surface" int_marker.header.frame_id = "base_link" int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id="base_link"), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose, ) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker( type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id="base_link"), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)), ) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker @staticmethod def get_frame_list(): """Function that returns the list of ref. frames""" objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) return objects @staticmethod def has_objects(): """Function that checks if there are any objects""" return len(World.objects) > 0 @staticmethod def object_dissimilarity(obj1, obj2): """Distance between two objects""" dims1 = obj1.dimensions dims2 = obj2.dimensions return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z])) @staticmethod def get_ref_from_name(ref_name): """Ref. frame type from ref. frame name""" if ref_name == "base_link": return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): """Transforms an arm frame to a new ref. frame""" if ref_frame == ArmState.ROBOT_BASE: if arm_frame.refFrame == ArmState.ROBOT_BASE: pass # rospy.logwarn('No reference frame transformations ' + #'needed (both absolute).') elif arm_frame.refFrame == ArmState.OBJECT: abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, "base_link") arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr( "Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame) ) elif ref_frame == ArmState.OBJECT: if arm_frame.refFrame == ArmState.ROBOT_BASE: rel_ee_pose = World.transform(arm_frame.ee_pose, "base_link", ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif arm_frame.refFrame == ArmState.OBJECT: if arm_frame.refFrameObject.name == ref_frame_obj.name: pass # rospy.logwarn('No reference frame transformations ' + #'needed (same object).') else: rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr( "Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame) ) return arm_frame @staticmethod def has_object(object_name): """Checks if the world contains the object""" for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): """Checks if the frame is valid for transforms""" return (object_name == "base_link") or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): """ Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms""" if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr("TF exception during transform.") return pose except rospy.ServiceException: rospy.logerr("Exception during transform.") return pose else: rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame) return pose @staticmethod def pose_to_string(pose): """For printing a pose to stdout""" return ( "Position: " + str(pose.position.x) + ", " + str(pose.position.y) + ", " + str(pose.position.z) + "\n" + "Orientation: " + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ", " + str(pose.orientation.z) + ", " + str(pose.orientation.w) + "\n" ) def _publish_tf_pose(self, pose, name, parent): """ Publishes a TF for object pose""" if pose != None: pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): """ Function to externally update an object pose""" Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while ( Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE ): time.sleep(0.1) if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not look down to take table snapshot") return False rospy.loginfo("Looking at table now.") goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Object recognition has been reset.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) self._reset_objects() if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not reset recognition.") return False # Do segmentation goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Table segmentation is complete.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not segment.") return False # Do recognition goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Objects on the table have been recognized.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) # Record the result if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: wait_time = 0 total_wait_time = 5 while not World.has_objects() and wait_time < total_wait_time: time.sleep(0.1) wait_time += 0.1 if not World.has_objects(): rospy.logerr("Timeout waiting for a recognition result.") return False else: rospy.loginfo("Got the object list.") return True else: rospy.logerr("Could not recognize.") return False def clear_all_objects(self): """Removes all objects from the world""" goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Object recognition has been reset.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo("Successfully reset object localization pipeline.") self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): """Gives a pointed to the nearest object""" dist_threshold = 0.4 def chObj(cur, obj): dist = World.pose_distance(obj.object.pose, arm_pose) return (dist, obj.object) if (dist < cur[0]) else cur return reduce(chObj, World.objects, (dist_threshold, None))[1] @staticmethod def pose_distance(pose1, pose2, is_on_table=True): """Distance between two world poses""" if pose1 == [] or pose2 == []: return 0.0 else: if is_on_table: arr1 = array([pose1.position.x, pose1.position.y]) arr2 = array([pose2.position.x, pose2.position.y]) else: arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z]) arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z]) dist = norm(arr1 - arr2) if dist < 0.0001: dist = 0 return dist def marker_feedback_cb(self, feedback): """Callback for when feedback from a marker is received""" if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo("Clicked on object " + str(feedback.marker_name)) rospy.loginfo("Number of objects " + str(len(World.objects))) else: rospy.loginfo("Unknown event" + str(feedback.event_type)) def update(self): """Update function called in a loop""" # Visualize the detected object is_world_changed = False self._lock.acquire() if World.has_objects(): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), "base_link") if World.objects[i].is_removed: to_remove = i if to_remove != None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed
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 InteractivePose2DTF(Node): """ A node that provides an interactive draggable marker, which publishes a tf """ pub_marker = Publisher('/marker', MarkerArray, queue_size=1) world_frame = Param(str, default='map') target_frame = Param(str, default='base_link') def __init__(self): self.transform = None super(InteractivePose2DTF, self).__init__() self.pub_tf = tf2_ros.TransformBroadcaster() self.interact = InteractiveMarkerServer("interactive_markers") self.marker_pose = Pose() self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5)) def _set_marker(self, color): self.interact.clear() marker = InteractiveMarker( header=Header(frame_id=self.world_frame), scale=1, name="Robot", controls=[ InteractiveMarkerControl( interaction_mode=InteractiveMarkerControl.MOVE_ROTATE, orientation=Quaternion( *transformations.quaternion_from_euler( 0, np.pi / 2, 0)), markers=[ Marker(type=Marker.ARROW, scale=Vector3(0.5, 0.05, 0.05), color=color, pose=Pose(orientation=Quaternion(0, 0, 0, 1))) ]) ], pose=self.marker_pose) self.interact.insert(marker, self.processFeedback) self.interact.applyChanges() def processFeedback(self, feedback): if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE: return self.marker_pose = feedback.pose @Timer(rospy.Duration(0.1)) def timer_tf_pub(self, event): t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = self.world_frame t.child_frame_id = self.target_frame t.transform.translation = self.marker_pose.position t.transform.rotation = self.marker_pose.orientation self.pub_tf.sendTransform(t) @Subscriber('~color', ColorRGBA) def sub_color(self, color): self._set_marker(color) @Subscriber('/initialpose', PoseWithCovarianceStamped) def sub_initialpose(self, pose): """ The default topic used by rviz """ self.marker_pose = pose.pose.pose self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
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 InteractivePose2DTF(Node): """ A node that provides an interactive draggable marker, which publishes a tf """ pub_marker = Publisher('/marker', MarkerArray, queue_size=1) world_frame = Param(str, default='map') target_frame = Param(str, default='base_link') def __init__(self): self.transform = None super(InteractivePose2DTF, self).__init__() self.pub_tf = tf2_ros.TransformBroadcaster() self.interact = InteractiveMarkerServer("interactive_markers") self.marker_pose = Pose() self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5)) def _set_marker(self, color): self.interact.clear() marker = InteractiveMarker( header=Header( frame_id=self.world_frame ), scale=1, name="Robot", controls=[ InteractiveMarkerControl( interaction_mode=InteractiveMarkerControl.MOVE_ROTATE, orientation=Quaternion(*transformations.quaternion_from_euler(0, np.pi/2, 0)), markers=[ Marker( type=Marker.ARROW, scale=Vector3(0.5, 0.05, 0.05), color=color, pose=Pose( orientation=Quaternion(0, 0, 0, 1) ) ) ] ) ], pose=self.marker_pose ) self.interact.insert(marker, self.processFeedback) self.interact.applyChanges() def processFeedback(self, feedback): if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE: return self.marker_pose = feedback.pose @Timer(rospy.Duration(0.1)) def timer_tf_pub(self, event): t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = self.world_frame t.child_frame_id = self.target_frame t.transform.translation = self.marker_pose.position t.transform.rotation = self.marker_pose.orientation self.pub_tf.sendTransform(t) @Subscriber('~color', ColorRGBA) def sub_color(self, color): self._set_marker(color) @Subscriber('/initialpose', PoseWithCovarianceStamped) def sub_initialpose(self, pose): """ The default topic used by rviz """ self.marker_pose = pose.pose.pose self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
class World: '''Handles object recognition, localization, and coordinate space transformations.''' tf_listener = None # Type: [WorldObject] objects = [] def __init__(self): # Public attributes if World.tf_listener is None: World.tf_listener = TransformListener() self.surface = None # Private attributes self._lock = threading.Lock() self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) rospy.wait_for_service(SERVICE_BB) self._bb_service = rospy.ServiceProxy( SERVICE_BB, FindClusterBoundingBox) self._object_action_client = actionlib.SimpleActionClient( ACTION_OBJ_DETECTION, UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo( 'Interactive object detection action server has responded.') # Setup other ROS machinery rospy.Subscriber( TOPIC_OBJ_RECOGNITION, GraspableObjectList, self.receive_object_info) rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker) # Init self.clear_all_objects() # ################################################################## # Static methods: Public (API) # ################################################################## @staticmethod def get_pose_from_transform(transform): '''Returns pose for transformation matrix. Args: transform (Matrix3x3): (I think this is the correct type. See ActionStepMarker as a reference for how to use.) Returns: Pose ''' pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose( Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]) ) @staticmethod def get_matrix_from_pose(pose): '''Returns the transformation matrix for given pose. Args: pose (Pose) Returns: Matrix3x3: (I think this is the correct type. See ActionStepMarker as a reference for how to use.) ''' pp, po = pose.position, pose.orientation rotation = [po.x, po.y, po.z, po.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pp.x, pp.y, pp.z] transformation[:3, 3] = position return transformation @staticmethod def get_absolute_pose(arm_state): '''Returns absolute pose of an end effector state (trasnforming if relative). Args: arm_state (ArmState) Returns: Pose ''' if arm_state.refFrame == ArmState.OBJECT: arm_state_copy = ArmState( arm_state.refFrame, Pose( arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def get_most_similar_obj(ref_object, ref_frame_list): '''Finds the most similar object in the world. Args: ref_object (?) ref_frame_list ([Object]): List of objects (as defined by Object.msg). Returns: Object|None: As in one of Object.msg, or None if no object was found close enough. ''' best_dist = 10000 # Not a constant; an absurdly high number. chosen_obj = None for ref_frame in ref_frame_list: dist = World.object_dissimilarity(ref_frame, ref_object) if dist < best_dist: best_dist = dist chosen_obj = ref_frame if chosen_obj is None: rospy.loginfo('Did not find a similar object.') else: rospy.loginfo('Object dissimilarity is --- ' + str(best_dist)) if best_dist > OBJ_SIMILAR_DIST_THRESHHOLD: rospy.loginfo('Found some objects, but not similar enough.') chosen_obj = None else: rospy.loginfo( 'Most similar to new object: ' + str(chosen_obj.name)) # Regardless, return the "closest object," which may be None. return chosen_obj @staticmethod def get_frame_list(): '''Function that returns the list of reference frames (Objects). Returns: [Object]: List of Object (as defined by Object.msg), the current reference frames. ''' return [w_obj.object for w_obj in World.objects] @staticmethod def has_objects(): '''Returns whetehr there are any objects (reference frames). Returns: bool ''' return len(World.objects) > 0 @staticmethod def object_dissimilarity(obj1, obj2): '''Returns distance between two objects. Returns: float ''' d1 = obj1.dimensions d2 = obj2.dimensions return norm(array([d1.x, d1.y, d1.z]) - array([d2.x, d2.y, d2.z])) @staticmethod def get_ref_from_name(ref_name): '''Returns the reference frame type from the reference frame name specified by ref_name. Args: ref_name (str): Name of a referene frame. Returns: int: One of ArmState.*, the number code of the reference frame specified by ref_name. ''' if ref_name == 'base_link': return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): '''Transforms an arm frame to a new ref. frame. Args: arm_frame (ArmState) ref_frame (int): One of ArmState.* ref_frame_obj (Object): As in Object.msg Returns: ArmState: arm_frame (passed in), but modified. ''' if ref_frame == ArmState.ROBOT_BASE: if arm_frame.refFrame == ArmState.ROBOT_BASE: # Transform from robot base to itself (nothing to do). rospy.logdebug( 'No reference frame transformations needed (both ' + 'absolute).') elif arm_frame.refFrame == ArmState.OBJECT: # Transform from object to robot base. abs_ee_pose = World.transform( arm_frame.ee_pose, arm_frame.refFrameObject.name, 'base_link' ) arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr( 'Unhandled reference frame conversion: ' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) elif ref_frame == ArmState.OBJECT: if arm_frame.refFrame == ArmState.ROBOT_BASE: # Transform from robot base to object. rel_ee_pose = World.transform( arm_frame.ee_pose, 'base_link', ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif arm_frame.refFrame == ArmState.OBJECT: # Transform between the same object (nothign to do). if arm_frame.refFrameObject.name == ref_frame_obj.name: rospy.logdebug( 'No reference frame transformations needed (same ' + 'object).') else: # Transform between two different objects. rel_ee_pose = World.transform( arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name ) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr( 'Unhandled reference frame conversion: ' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) return arm_frame @staticmethod def has_object(object_name): '''Returns whether the world contains an Object with object_name. Args: object_name (str) Returns: bool ''' return object_name in [wobj.object.name for wobj in World.objects] @staticmethod def is_frame_valid(object_name): '''Returns whether the frame (object) name is valid for transforms. Args: object_name (str) Returns: bool ''' return object_name == 'base_link' or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): '''Transforms a pose between two reference frames. If there is a TF exception or object does not exist, it will return the pose back without any transforms. Args: pose (Pose) from_frame (str) to_frame (str) Returns: Pose ''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime( from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose( to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('ServiceException during transform.') return pose else: rospy.logdebug( 'One of the frame objects might not exist: ' + from_frame + ' or ' + to_frame) return pose @staticmethod def pose_distance(pose1, pose2, is_on_table=True): '''Returns distance between two world poses. Args: pose1 (Pose) pose2 (Pose) is_on_table (bool, optional): Whether the objects are on the table (if so, disregards z-values in computations). Returns: float ''' if pose1 == [] or pose2 == []: return 0.0 else: p1p = pose1.position p2p = pose2.position if is_on_table: arr1 = array([p1p.x, p1p.y]) arr2 = array([p2p.x, p2p.y]) else: arr1 = array([p1p.x, p1p.y, p1p.z]) arr2 = array([p2p.x, p2p.y, p2p.z]) dist = norm(arr1 - arr2) if dist < OBJ_DIST_ZERO_CLAMP: dist = 0 return dist @staticmethod def log_pose(log_fn, pose): '''For printing a pose to rosout. We don't do it on one line becuase that messes up the indentation with the rest of the log. Args: log_fn (function(str)): A logging function that takes a string as an argument. For example, rospy.loginfo. pose (Pose): The pose to log ''' p, o = pose.position, pose.orientation log_fn(' - position: (%f, %f, %f)' % (p.x, p.y, p.z)) log_fn(' - orientation: (%f, %f, %f, %f)' % (o.x, o.y, o.z, o.w)) # ################################################################## # Static methods: Internal ("private") # ################################################################## @staticmethod def _get_mesh_marker(marker, mesh): '''Generates and returns a marker from a mesh. Args: marker (Marker) mesh (Mesh) Returns: Marker ''' marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while index + 2 < len(mesh.triangles): if (mesh.triangles[index] < len(mesh.vertices) and mesh.triangles[index + 1] < len(mesh.vertices) and mesh.triangles[index + 2] < len(mesh.vertices)): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr('Mesh contains invalid triangle!') break return marker @staticmethod def _get_surface_marker(pose, dimensions): '''Returns a surface marker with provided pose and dimensions. Args: pose (Pose) dimensions (Vector3) Returns: InteractiveMarker ''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = BASE_LINK int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=2000, lifetime=MARKER_DURATION, scale=dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_SURFACE, pose=pose ) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker( type=Marker.TEXT_VIEW_FACING, id=2001, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker # ################################################################## # Instance methods: Public (API) # ################################################################## def receive_table_marker(self, marker): '''Callback function for markers to determine table''' if marker.type == Marker.LINE_STRIP: if len(marker.points) == 6: rospy.loginfo('Received a TABLE marker.') xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, SURFACE_HEIGHT) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert( self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receive_object_info(self, object_list): '''Callback function to receive object info''' self._lock.acquire() rospy.loginfo('Received recognized object list.') if len(object_list.graspable_objects) > 0: for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if len(models) > 0: object_pose = None best_confidence = 0.0 for j in range(len(models)): if best_confidence < models[j].confidence: object_pose = models[j].pose.pose best_confidence = models[j].confidence if object_pose is not None: rospy.logwarn( 'Adding the recognized object with most ' + 'confident model.') self._add_new_object( object_pose, DIMENSIONS_OBJ, True, object_list.meshes[i] ) else: rospy.logwarn( '... this is not a recognition result, it is ' + 'probably just segmentation.') cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if cluster_pose is not None: rospy.loginfo('Adding unrecognized object with pose:') World.log_pose(rospy.loginfo, cluster_pose) rospy.loginfo( '...in ref frame ' + str(bbox.pose.header.frame_id)) self._add_new_object( cluster_pose, bbox.box_dims, False) else: rospy.logwarn('... but the list was empty.') self._lock.release() def update_object_pose(self): ''' Function to externally update an object pose.''' # Look down at the table. rospy.loginfo('Head attempting to look at table.') Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while (Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE): rospy.sleep(PAUSE_SECONDS) if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr('Could not look down to take table snapshot') return False rospy.loginfo('Head is now (successfully) stairing at table.') # Reset object recognition. rospy.loginfo('About to attempt to reset object recognition.') goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): rospy.sleep(PAUSE_SECONDS) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) self._reset_objects() # Also do this internally. if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr('Could not reset recognition.') return False # Do segmentation rospy.loginfo('About to attempt table segmentation.') goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): rospy.sleep(PAUSE_SECONDS) rospy.loginfo('Table segmentation is complete.') rospy.loginfo( 'STATUS: ' + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logwarn('Could not segment.') return False # Do recognition rospy.loginfo('About to attempt object recognition.') goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): rospy.sleep(PAUSE_SECONDS) rospy.loginfo('Objects on the table have been recognized.') rospy.loginfo( 'STATUS: ' + self._object_action_client.get_goal_status_text()) # Record the result if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: wait_time = rospy.Duration(0.0) while (not World.has_objects() and wait_time < RECOGNITION_TIMEOUT_SECONDS): rospy.sleep(PAUSE_SECONDS) wait_time += PAUSE_SECONDS if not World.has_objects(): rospy.logerr('Timeout waiting for a recognition result.') return False else: rospy.loginfo('Got the object list.') return True else: rospy.logerr('Could not recognize.') return False def clear_all_objects(self): '''Removes all objects from the world.''' goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): rospy.sleep(PAUSE_SECONDS) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Successfully reset object localization pipeline.') self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): '''Returns the nearest object, if one exists. Args: arm_pose (Pose): End-effector pose. Returns: Object|None: As in Object.msg, the nearest object (if it is close enough), or None if there were none close enough. ''' # First, find which object is the closest. distances = [] for wobj in World.objects: dist = World.pose_distance(wobj.object.pose, arm_pose) distances.append(dist) # Then, see if the closest is actually below our threshhold for # a 'closest object.' if len(distances) > 0: if min(distances) < OBJ_NEAREST_DIST_THRESHHOLD: chosen = distances.index(min(distances)) return World.objects[chosen].object # We didn't have any objects or none were close enough. return None def marker_feedback_cb(self, feedback): '''Callback for when feedback from a marker is received. Args: feedback (InteractiveMarkerFeedback) ''' if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Clicked on object ' + str(feedback.marker_name)) rospy.loginfo('Number of objects ' + str(len(World.objects))) else: # This happens a ton, and doesn't need to be logged like # normal events (e.g. clicking on most marker controls # fires here). rospy.logdebug('Unknown event: ' + str(feedback.event_type)) def update(self): '''Update function called in a loop. Returns: bool: Whether any tracked objects were removed, AKA "is world changed." ''' # Visualize the detected object is_world_changed = False self._lock.acquire() if World.has_objects(): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose( World.objects[i].object.pose, World.objects[i].get_name(), BASE_LINK ) if World.objects[i].is_removed: to_remove = i if to_remove is not None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed # ################################################################## # Instance methods: Internal ("private") # ################################################################## def _reset_objects(self): '''Removes all objects.''' self._lock.acquire() for wobj in World.objects: self._im_server.erase(wobj.int_marker.name) self._im_server.applyChanges() if self.surface is not None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def _add_new_object(self, pose, dimensions, is_recognized, mesh=None): '''Maybe add a new object with the specified properties to our object list. It might not be added if too similar of an object already exists (and has been added). Args: pose (Pose) dimensions (Vector3) is_recognized (bool) mesh (Mesh, optional): A mesh, if it exists. Default is None. Returns: bool: Whether the object was actually added. ''' to_remove = None if is_recognized: # TODO(mbforbes): Re-implement object recognition or remove # this dead code. return False # # Check if there is already an object # for i in range(len(World.objects)): # distance = World.pose_distance( # World.objects[i].object.pose, pose) # if distance < OBJ_ADD_DIST_THRESHHOLD: # if World.objects[i].is_recognized: # rospy.loginfo( # 'Previously recognized object at the same ' + # 'location, will not add this object.') # return False # else: # rospy.loginfo( # 'Previously unrecognized object at the same ' + # 'location, will replace it with the recognized '+ # 'object.') # to_remove = i # break # # Remove any duplicate objects. # if to_remove is not None: # self._remove_object(to_remove) # # Actually add the object. # self._add_new_object_internal( # pose, dimensions, is_recognized, mesh) # return True else: # Whether whether we already have an object at ~ the same # location (and if so, don't add). for wobj in World.objects: if (World.pose_distance(wobj.object.pose, pose) < OBJ_ADD_DIST_THRESHHOLD): rospy.loginfo( 'Previously detected object at the same location, ' + 'will not add this object.') return False # Actually add the object. self._add_new_object_internal( pose, dimensions, is_recognized, mesh) return True def _add_new_object_internal(self, pose, dimensions, is_recognized, mesh): '''Does the 'internal' adding of an object with the passed properties. Call _add_new_object to do all pre-requisite checks first (it then calls this function). Args: pose (Pose) dimensions (Vector3) is_recognized (bool) mesh (Mesh|None): A mesh, if it exists (can be None). ''' n_objects = len(World.objects) World.objects.append(WorldObject( pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply( self._im_server, int_marker.name) self._im_server.applyChanges() def _remove_object(self, to_remove): '''Remove an object by index. Args: to_remove (int): Index of the object to remove in World.objects. ''' obj = World.objects.pop(to_remove) rospy.loginfo('Removing object ' + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() # TODO(mbforbes): Re-implement object recognition or remove # this dead code. # if (obj.is_recognized): # for i in range(len(World.objects)): # if ((World.objects[i].is_recognized) # and World.objects[i].index > obj.index): # World.objects[i].decrease_index() # self.n_recognized -= 1 # else: # for i in range(len(World.objects)): # if ((not World.objects[i].is_recognized) and # World.objects[i].index > obj.index): # World.objects[i].decrease_index() # self.n_unrecognized -= 1 def _remove_surface(self): '''Function to request removing surface (from IM).''' rospy.loginfo('Removing surface') self._im_server.erase('surface') self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): '''Generate and return a marker for world objects. Args: index (int): ID for the new marker. mesh (Mesh, optional): Mesh to use for the marker. Only utilized if not None. Defaults to None. Returns: InteractiveMarker ''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=index, lifetime=MARKER_DURATION, scale=World.objects[index].object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=World.objects[index].object.pose ) if mesh is not None: object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = ( World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=index, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) ) int_marker.controls.append(button_control) return int_marker def _publish_tf_pose(self, pose, name, parent): ''' Publishes a TF for object named name with pose pose and parent reference frame parent. Args: pose (Pose): The object's pose. name (str): The object's name. parent (str): The parent reference frame. ''' if pose is not None: pp = pose.position po = pose.orientation pos = (pp.x, pp.y, pp.z) rot = (po.x, po.y, po.z, po.w) # TODO(mbforbes): Is it necessary to change the position # and orientation into tuples to send to TF? self._tf_broadcaster.sendTransform( pos, rot, rospy.Time.now(), name, parent)
class World: '''Object recognition and localization related stuff''' tf_listener = None objects = [] def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') bb_service_name = 'find_cluster_bounding_box' rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber('interactive_object_recognition_result', GraspableObjectList, self.receieve_object_info) self._object_action_client = actionlib.SimpleActionClient( 'object_detection_user_command', UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo('Interactive object detection action ' + 'server has responded.') self.clear_all_objects() # The following is to get the table information rospy.Subscriber('tabletop_segmentation_markers', Marker, self.receieve_table_marker) def _reset_objects(self): '''Function that removes all objects''' self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def receieve_table_marker(self, marker): '''Callback function for markers to determine table''' if (marker.type == Marker.LINE_STRIP): if (len(marker.points) == 6): rospy.loginfo('Received a TABLE marker.') xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receieve_object_info(self, object_list): '''Callback function to receive object info''' self._lock.acquire() rospy.loginfo('Received recognized object list.') if (len(object_list.graspable_objects) > 0): for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if (len(models) > 0): object_pose = None best_confidence = 0.0 for j in range(len(models)): if (best_confidence < models[j].confidence): object_pose = models[j].pose.pose best_confidence = models[j].confidence if (object_pose != None): rospy.logwarn('Adding the recognized object ' + 'with most confident model.') self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i]) else: rospy.logwarn('... this is not a recognition result, ' + 'it is probably just segmentation.') cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if (cluster_pose != None): rospy.loginfo('Adding unrecognized object with pose:' + World.pose_to_string(cluster_pose) + '\n' + 'In ref frame' + str(bbox.pose.header.frame_id)) self._add_new_object(cluster_pose, bbox.box_dims, False) else: rospy.logwarn('... but the list was empty.') self._lock.release() @staticmethod def get_pose_from_transform(transform): '''Returns pose for transformation matrix''' pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) @staticmethod def get_matrix_from_pose(pose): '''Returns the transformation matrix for given pose''' rotation = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation @staticmethod def get_absolute_pose(arm_state): '''Returns absolute pose of an end effector state''' if (arm_state.refFrame == ArmState.OBJECT): arm_state_copy = ArmState( arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def get_most_similar_obj(ref_object, ref_frame_list): '''Finds the most similar object in the world''' best_dist = 10000 chosen_obj_index = -1 for i in range(len(ref_frame_list)): dist = World.object_dissimilarity(ref_frame_list[i], ref_object) if (dist < best_dist): best_dist = dist chosen_obj_index = i if chosen_obj_index == -1: rospy.logwarn('Did not find a similar object..') return None else: print 'Object dissimilarity is --- ', best_dist if best_dist > 0.075: rospy.logwarn('Found some objects, but not similar enough.') return None else: rospy.loginfo('Most similar to new object ' + str(chosen_obj_index)) return ref_frame_list[chosen_obj_index] @staticmethod def _get_mesh_marker(marker, mesh): '''Function that generated a marker from a mesh''' marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while (index + 2 < len(mesh.triangles)): if ((mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices))): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr('Mesh contains invalid triangle!') break return marker def _add_new_object(self, pose, dimensions, is_recognized, mesh=None): '''Function to add new objects''' dist_threshold = 0.02 to_remove = None if (is_recognized): # Temporary HACK for testing. # Will remove all recognition completely if this works. return False # Check if there is already an object for i in range(len(World.objects)): distance = World.pose_distance(World.objects[i].object.pose, pose) if (distance < dist_threshold): if (World.objects[i].is_recognized): rospy.loginfo( 'Previously recognized object at ' + 'the same location, will not add this object.') return False else: rospy.loginfo( 'Previously unrecognized object at ' + 'the same location, will replace it with the ' + 'recognized object.') to_remove = i break if (to_remove != None): self._remove_object(to_remove) n_objects = len(World.objects) World.objects.append( WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1, mesh) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True else: for i in range(len(World.objects)): if (World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold): rospy.loginfo('Previously detected object at the same' + 'location, will not add this object.') return False n_objects = len(World.objects) World.objects.append( WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _remove_object(self, to_remove): '''Function to remove object by index''' obj = World.objects.pop(to_remove) rospy.loginfo('Removing object ' + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() # if (obj.is_recognized): # for i in range(len(World.objects)): # if ((World.objects[i].is_recognized) # and World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_recognized -= 1 # else: # for i in range(len(World.objects)): # if ((not World.objects[i].is_recognized) and # World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_unrecognized -= 1 def _remove_surface(self): '''Function to request removing surface''' rospy.loginfo('Removing surface') self._im_server.erase('surface') self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker @staticmethod def get_frame_list(): '''Function that returns the list of ref. frames''' objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) return objects @staticmethod def has_objects(): '''Function that checks if there are any objects''' return len(World.objects) > 0 @staticmethod def object_dissimilarity(obj1, obj2): '''Distance between two objects''' dims1 = obj1.dimensions dims2 = obj2.dimensions return norm( array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z])) @staticmethod def get_ref_from_name(ref_name): '''Ref. frame type from ref. frame name''' if ref_name == 'base_link': return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): '''Transforms an arm frame to a new ref. frame''' if ref_frame == ArmState.ROBOT_BASE: if (arm_frame.refFrame == ArmState.ROBOT_BASE): pass #rospy.logwarn('No reference frame transformations ' + #'needed (both absolute).') elif (arm_frame.refFrame == ArmState.OBJECT): abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, 'base_link') arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) elif ref_frame == ArmState.OBJECT: if (arm_frame.refFrame == ArmState.ROBOT_BASE): rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link', ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif (arm_frame.refFrame == ArmState.OBJECT): if (arm_frame.refFrameObject.name == ref_frame_obj.name): pass #rospy.logwarn('No reference frame transformations ' + #'needed (same object).') else: rel_ee_pose = World.transform( arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) return arm_frame @staticmethod def has_object(object_name): '''Checks if the world contains the object''' for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): '''Checks if the frame is valid for transforms''' return (object_name == 'base_link') or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): ''' Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime( from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose( to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('Exception during transform.') return pose else: rospy.logwarn('One of the frame objects might not exist: ' + from_frame + ' or ' + to_frame) return pose @staticmethod def pose_to_string(pose): '''For printing a pose to stdout''' return ('Position: ' + str(pose.position.x) + ", " + str(pose.position.y) + ', ' + str(pose.position.z) + '\n' + 'Orientation: ' + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ', ' + str(pose.orientation.z) + ', ' + str(pose.orientation.w) + '\n') def _publish_tf_pose(self, pose, name, parent): ''' Publishes a TF for object pose''' if (pose != None): pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): ''' Function to externally update an object pose''' Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while (Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE): time.sleep(0.1) if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not look down to take table snapshot') return False rospy.loginfo('Looking at table now.') goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) self._reset_objects() if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not reset recognition.') return False # Do segmentation goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Table segmentation is complete.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not segment.') return False # Do recognition goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Objects on the table have been recognized.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) # Record the result if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): wait_time = 0 total_wait_time = 5 while (not World.has_objects() and wait_time < total_wait_time): time.sleep(0.1) wait_time += 0.1 if (not World.has_objects()): rospy.logerr('Timeout waiting for a recognition result.') return False else: rospy.loginfo('Got the object list.') return True else: rospy.logerr('Could not recognize.') return False def clear_all_objects(self): '''Removes all objects from the world''' goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('Successfully reset object localization pipeline.') self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): '''Gives a pointed to the nearest object''' dist_threshold = 0.4 def chObj(cur, obj): dist = World.pose_distance(obj.object.pose, arm_pose) return (dist, obj.object) if (dist < cur[0]) else cur return reduce(chObj, World.objects, (dist_threshold, None))[1] @staticmethod def pose_distance(pose1, pose2, is_on_table=True): '''Distance between two world poses''' if pose1 == [] or pose2 == []: return 0.0 else: if (is_on_table): arr1 = array([pose1.position.x, pose1.position.y]) arr2 = array([pose2.position.x, pose2.position.y]) else: arr1 = array( [pose1.position.x, pose1.position.y, pose1.position.z]) arr2 = array( [pose2.position.x, pose2.position.y, pose2.position.z]) dist = norm(arr1 - arr2) if dist < 0.0001: dist = 0 return dist def marker_feedback_cb(self, feedback): '''Callback for when feedback from a marker is received''' if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Clicked on object ' + str(feedback.marker_name)) rospy.loginfo('Number of objects ' + str(len(World.objects))) else: rospy.loginfo('Unknown event' + str(feedback.event_type)) def update(self): '''Update function called in a loop''' # Visualize the detected object is_world_changed = False self._lock.acquire() if (World.has_objects()): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), 'base_link') if (World.objects[i].is_removed): to_remove = i if to_remove != None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed
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()
class SkirosInteractiveMarkers: default_box_size = 0.1 def on_marker_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 print s print mp def _make_box(self, msg, size): marker = Marker() marker.type = Marker.CUBE if None in size: size = [SkirosInteractiveMarkers.default_box_size, SkirosInteractiveMarkers.default_box_size, SkirosInteractiveMarkers.default_box_size] marker.scale.x = msg.scale * size[0] marker.scale.y = msg.scale * size[1] marker.scale.z = msg.scale * size[2] marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 0.5 marker.color.a = 0.5 return marker def _make_box_control(self, msg, size): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self._make_box(msg, size)) msg.controls.append(control) return control def initInteractiveServer(self, name): """ @brief Start the interactive marker server """ self._server = InteractiveMarkerServer(name) def clear_markers(self): self._server.clear() def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode): int_marker = InteractiveMarker() int_marker.header.frame_id = base_frame_id int_marker.pose.position.x = pose[0][0] int_marker.pose.position.y = pose[0][1] int_marker.pose.position.z = pose[0][2] int_marker.pose.orientation.x = pose[1][0] int_marker.pose.orientation.y = pose[1][1] int_marker.pose.orientation.z = pose[1][2] int_marker.pose.orientation.w = pose[1][3] int_marker.scale = 1 int_marker.name = frame_id int_marker.description = frame_id # insert a box self._make_box_control(int_marker, size) int_marker.controls[0].interaction_mode = interaction_mode n = norm([1, 1]) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 1 / n 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 / n control.orientation.x = 1 / n 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 / n control.orientation.x = 0 control.orientation.y = 1 / n control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 1 / n control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 / n control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 / n control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self._server.insert(int_marker, self.on_marker_feedback) self._server.applyChanges()