class ParkingSpotServer(Node): def __init__(self): super(ParkingSpotServer, self).__init__('parking_spot_server') self.declare_parameter('map_yaml') self.markers = {} self.poses = {} self.marker_server = InteractiveMarkerServer(self, 'markers') map_param = self.get_parameter('map_yaml').value self.map_path = Path(map_param) self.load_map() self.name_counter = 0 # Create a timeout to throttle saving data on feedback # We reset it on marker feedback so that once the user stops editing, save is triggered self.save_timer = self.create_timer(0.1, self.save_map) self.save_timer.cancel() self.add_srv = self.create_service(AddParkingSpot, 'add_parking_spot', self.add_spot) self.del_srv = self.create_service(DeleteParkingSpot, 'delete_parking_spot', self.delete_spot) self.get_srv = self.create_service(GetParkingSpot, 'get_parking_spot', self.get_spot) self.rename_srv = self.create_service(RenameParkingSpot, 'rename_parking_spot', self.rename_spot) def save_map(self): self.save_timer.cancel() dump = {} for name, marker in self.markers.items(): pose = marker.pose euler = euler_from_quaternion([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) yaw = euler[2] dump[name] = [ pose.position.x, pose.position.y, yaw, ] self.map_data['parking'] = dump with self.map_path.open('w') as map_yaml: yaml.dump(self.map_data, map_yaml) def load_map(self): with self.map_path.open('r') as map_yaml: self.map_data = yaml.safe_load(map_yaml) self.poses = { name: Pose2D(x=pose[0], y=pose[1], theta=pose[2]) for name, pose in self.map_data.get('parking', {}).items() } self.markers = { name: self.add_marker(name, Pose2D(x=pose[0], y=pose[1], theta=pose[2])) for name, pose in self.map_data.get('parking', {}).items() } def marker_feedback(self, event: InteractiveMarkerFeedback) -> None: self.save_timer.reset() def add_marker(self, name: str, pose: Pose2D) -> InteractiveMarker: marker = InteractiveMarker(name=name, pose=Pose( position=Point(x=pose.x, y=pose.y, z=0.), orientation=quaternion_from_euler( pose.theta, 0., 0.), )) marker.header.frame_id = 'map' sc = 0.2 z = 0.1 name_marker = Marker( type=Marker.TEXT_VIEW_FACING, scale=Vector3(x=sc, y=sc, z=sc), color=ColorRGBA(r=1., g=1., b=1., a=1.), text=name, ) name_marker.pose.position.x = sc * -0.1 name_marker.pose.position.z = z + sc * -0.1 marker.controls = [ InteractiveMarkerControl( name='name', orientation_mode=InteractiveMarkerControl.VIEW_FACING, interaction_mode=InteractiveMarkerControl.NONE, independent_marker_orientation=False, always_visible=True, markers=[name_marker], ), InteractiveMarkerControl( name='xaxis', orientation_mode=InteractiveMarkerControl.FIXED, orientation=Quaternion(x=0., y=0., z=0.7068252, w=0.7068252), interaction_mode=InteractiveMarkerControl.MOVE_AXIS, ), InteractiveMarkerControl( name='yaxis', orientation_mode=InteractiveMarkerControl.FIXED, interaction_mode=InteractiveMarkerControl.MOVE_AXIS, ), InteractiveMarkerControl( name='turn', interaction_mode=InteractiveMarkerControl.ROTATE_AXIS, orientation=Quaternion(x=0., y=0.7068252, z=0., w=0.7068252), ) ] self.marker_server.insert(marker, feedback_callback=self.marker_feedback) self.marker_server.applyChanges() return marker def add_spot(self, request, response): print(request) name = 'park{:02}'.format(self.name_counter) while name in self.poses: self.name_counter += 1 name = 'park{:02}'.format(self.name_counter) marker = self.add_marker(name, request.pose) self.poses[name] = request.pose self.markers[name] = marker response.success = True self.save_map() return response def delete_spot(self, request, response): try: del self.poses[request.name] self.marker_server.erase(request.name) self.marker_server.applyChanges() del self.markers[request.name] response.success = True self.save_map() except KeyError: response.success = False return response def get_spot(self, request, response): try: response.pose = self.poses[request.name] response.success = True except KeyError: response.success = False return response def rename_spot(self, request, response): if request.name not in self.poses: response.success = False response.msg = 'Spot does not exist' elif request.new_name in self.poses: response.success = False response.msg = 'New name already taken' else: response.success = True name = request.name pose = self.poses[name] del self.markers[name] del self.poses[name] self.poses[request.new_name] = pose self.marker_server.erase(name) marker = self.add_marker(request.new_name, pose) self.markers[request.new_name] = marker self.save_map() return response
class AnnotatorServer(object): def __init__(self): self._annotator = Annotator() self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names", PoseNames, queue_size=10, latch=True) # self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses", # InteractiveMarker, queue_size=10, latch=True) self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses") self.INITIAL_POSE = Pose() self.INITIAL_POSE.orientation.w = 1 print("Initializing saved markers: " + str(self._annotator.get_position_names())) for name, pose in self._annotator.get_position_items(): self.__create_int_marker__(name, pose) self.__pub_pose_info__() print("Initialization finished...") def __pub_pose_info__(self): pose_names = PoseNames() pose_names.names = self._annotator.get_position_names() pose_names.names.sort() self._pose_names_pub.publish(pose_names) def __update_marker_pose__(self, input): if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP): name = input.marker_name new_pose = self._int_marker_server.get(name).pose # Overwrite the previous pose with the new pose self._annotator.save_position(name, new_pose) self._int_marker_server.setPose(name, new_pose) self._int_marker_server.applyChanges() print("updated pose for: " + name) def __create_int_marker__(self, name, pose): print("creating int marker: " + name) int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = name int_marker.description = name int_marker.pose = pose # Move it 0.25 meters up to make it easier to click int_marker.pose.position.z = 0.25 text_marker = Marker() text_marker.text = name text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.position.z = 2 text_marker.scale.x = 0.4 text_marker.scale.y = 0.4 text_marker.scale.z = 0.4 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1.0 text_control = InteractiveMarkerControl() text_control.name = "text_control" text_control.markers.append(text_marker) text_control.always_visible = True text_control.interaction_mode = InteractiveMarkerControl.NONE int_marker.controls.append(text_control) rotation_ring_control = InteractiveMarkerControl() rotation_ring_control.name = "position_control" rotation_ring_control.always_visible = True rotation_ring_control.orientation.x = 0 rotation_ring_control.orientation.w = 1 rotation_ring_control.orientation.y = 1 rotation_ring_control.orientation.z = 0 rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(rotation_ring_control) arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.pose.position.z = 0.15 arrow_marker.pose.position.x = -0.5 arrow_marker.scale.x = 1 arrow_marker.scale.y = 0.25 arrow_marker.scale.z = 0.25 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 position_control = InteractiveMarkerControl() position_control.name = "rotation_control" position_control.always_visible = True position_control.markers.append(arrow_marker) position_control.orientation.w = 1 position_control.orientation.x = 0 position_control.orientation.y = 1 position_control.orientation.z = 0 position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(position_control) self._int_marker_server.insert(int_marker, self.__update_marker_pose__) self._int_marker_server.applyChanges() def create(self, name, pose=None): print("creating new pose: " + name) if pose is None: pose = self.INITIAL_POSE self._annotator.save_position(name, pose) self.__create_int_marker__(name, pose) self.__pub_pose_info__() def delete(self, name): if self._annotator.exists(name): print("deleting pose: " + name) self._annotator.delete_position(name) self._int_marker_server.erase(name) self._int_marker_server.applyChanges() self.__pub_pose_info__() def handle_callback(self, user_action_msg): cmd = user_action_msg.command name = user_action_msg.name if cmd == UserAction.CREATE: self.create(name) elif cmd == UserAction.DELETE: self.delete(name) elif cmd == UserAction.GOTO: print("going to pose: " + name) self._annotator.goto_position(name)
class TrajectoryAnalyzer(): def __init__(self, marker_name): host = rospy.get_param("mongodb_host") port = rospy.get_param("mongodb_port") self._client = pymongo.MongoClient(host, port) self._traj = dict() self._retrieve_logs() self._server = InteractiveMarkerServer(marker_name) def get_poses_persecond(self): average_poses = 0 for uuid in self._traj: traj = self._traj[uuid] inner_counter = 1 outer_counter = 1 prev_sec = traj.secs[0] for i, sec in enumerate(traj.secs[1:]): if prev_sec == sec: inner_counter += 1 else: prev_sec = sec outer_counter += 1 average_poses += round(inner_counter/outer_counter) return round(average_poses/len(self._traj)) def _retrieve_logs(self): logs = self._client.message_store.people_perception.find() for log in logs: for i, uuid in enumerate(log['uuids']): if uuid not in self._traj: t = Trajectory(uuid) t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) self._traj[uuid] = t else: t = self._traj[uuid] t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) def visualize_trajectories(self, mode="all", average_length=0, longest_length=0): counter = 0 for uuid in self._traj: if len(self._traj[uuid].pose) > 1: if mode == "average": if abs(self._traj[uuid].length - average_length) \ < (average_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "longest": if abs(self._traj[uuid].length - longest_length) \ < (longest_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "shortest": if self._traj[uuid].length < 1: self.visualize_trajectory(self._traj[uuid]) counter += 1 else: self.visualize_trajectory(self._traj[uuid]) counter += 1 rospy.loginfo("Total Trajectories: " + str(len(self._traj))) rospy.loginfo("Printed trajectories: " + str(counter)) self.delete_trajectory(self._traj[uuid]) def _update_cb(self, feedback): return def visualize_trajectory(self, traj): int_marker = self.create_trajectory_marker(traj) self._server.insert(int_marker, self._update_cb) self._server.applyChanges() def delete_trajectory(self, traj): self._server.erase(traj.uuid) self._server.applyChanges() def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 3 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker def trajectory_visualization(self, mode): average_length = 0 longest_length = -1 short_trajectories = 0 average_max_vel = 0 highest_max_vel = -1 minimal_frame = self.get_poses_persecond() * 5 for k, v in self._traj.items(): v.sort_pose() v.calc_stats() # Delete non-moving objects if (v.max_vel < 0.1 or v.length < 0.1) and k in self._traj: del self._traj[k] # Delete trajectories that appear less than 5 seconds if len(v.pose) < minimal_frame and k in self._traj: del self._traj[k] for k, v in self._traj.iteritems(): average_length += v.length average_max_vel += v.max_vel if v.length < 1: short_trajectories += 1 if longest_length < v.length: longest_length = v.length if highest_max_vel < v.max_vel: highest_max_vel = v.max_vel average_length /= len(self._traj) average_max_vel /= len(self._traj) rospy.loginfo("Average length of tracks is " + str(average_length)) rospy.loginfo("Longest length of tracks is " + str(longest_length)) rospy.loginfo("Short trajectories are " + str(short_trajectories)) rospy.loginfo("Average maximum velocity of tracks is " + str(average_max_vel)) rospy.loginfo("Highest maximum velocity of tracks is " + str(highest_max_vel)) self.visualize_trajectories(mode, average_length, longest_length)
class ActuatorServer(object): def __init__(self): self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped) self.pose_pub = rospy.Publisher('/pose_names', order_system.msg.PoseNames, latch=True) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback) self.names = {} try: with open( "/home/team3/catkin_ws/src/cse481c/order_service/LOL.pickle", "r") as f: self.names = pickle.load(f) except EOFError: self.names = {} self.pose = None self.server = InteractiveMarkerServer('simple_marker') self.name_markers = {} self.name_controls = {} def _create_marker(self, name): self.name_markers[name] = InteractiveMarker() self.name_markers[name].header.frame_id = "map" self.name_markers[name].name = name self.name_markers[name].description = name self.name_markers[ name].pose.position.x = self.pose.pose.pose.position.x self.name_markers[ name].pose.position.y = self.pose.pose.pose.position.y # TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL self.name_markers[name].pose.position.z = 1 box_marker = create_box_marker() self.name_controls[name] = InteractiveMarkerControl() self.name_controls[ name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE self.name_controls[name].always_visible = True self.name_controls[name].orientation.w = 1 self.name_controls[name].orientation.x = 0 self.name_controls[name].orientation.y = 1 self.name_controls[name].orientation.z = 0 new_controller_lol = InteractiveMarkerControl() new_controller_lol.orientation.w = 1 new_controller_lol.orientation.x = 0 new_controller_lol.orientation.y = 1 new_controller_lol.orientation.z = 0 new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED self.name_markers[name].controls.append(new_controller_lol) # self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w # self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x # self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y # self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z self.name_controls[name].markers.append(box_marker) self.name_markers[name].controls.append(self.name_controls[name]) self.server.insert(self.name_markers[name], self._callback) self.server.applyChanges() def _callback(self, dog): name = dog.marker_name pose = dog.pose rospy.logerr(str(self.names[name])) self.names[name].pose.pose.position.x = pose.position.x self.names[name].pose.pose.position.y = pose.position.y self.names[name].pose.pose.orientation = pose.orientation rospy.logerr(str(self.names[name])) def _delete_marker(self, name): self.server.erase(name) self.server.applyChanges() def handle_send_fetch(self, request): rospy.logerr("yo we got fam" + str(request)) if request.command == request.CREATE: self._handle_create(request.name) self._create_marker(request.name) elif request.command == request.DELETE: self._handle_delete(request.name) self._delete_marker(request.name) elif request.command == request.GOTO: self._handle_goto(request.name) elif request.command == request.LIST: rospy.loginfo('hey Karen') else: rospy.logerr('none of these work') pose_message = order_system.msg.PoseNames() pose_message.poses = list(self.names.keys()) self.pose_pub.publish(pose_message) fetch_response = SendFetchResponse() fetch_response.names = list(self.names.keys()) return fetch_response def _handle_create(self, name): rospy.loginfo(str(self.pose)) self.names[name] = self.pose def _handle_delete(self, name): if name not in self.names: rospy.loginfo('wtf bro') else: del self.names[name] def _handle_goto(self, name): new_goto = geometry_msgs.msg.PoseStamped() rospy.loginfo(str(new_goto)) new_goto.header = self.names[name].header new_goto.pose = self.names[name].pose.pose rospy.logerr("wtf" + str(self.names[name].pose.pose)) rospy.logerr("bro" + str(new_goto)) self.goto_pub.publish(new_goto) def _handle_pose_callback(self, data): self.pose = copy.deepcopy(data) def pickle_it_up_bro(self): with open("/home/team3/catkin_ws/src/cse481c/map_annotator/LOL.pickle", "w") as f: pickle.dump(self.names, f)
class POIServer(object): ''' Node to act as a server to hold a list of points of interest which can be modified by services or interactive markers ''' def __init__(self): ''' Create a POIServer ''' # TF bootstrap self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # Radius of interactive marker for POIs self.marker_scale = rospy.get_param("~marker_scale", 0.5) # Create intial empty POI array self.pois = POIArray() # Get the global frame to be used self.global_frame = rospy.get_param("~global_frame", "map") self.pois.header.frame_id = self.global_frame # Create publisher to notify clients of updates and interactive marker server self.pub = rospy.Publisher("points_of_interest", POIArray, queue_size=1, latch=True) self.interactive_marker_server = InteractiveMarkerServer( "points_of_interest") # Load initial POIs from params if rospy.has_param('~initial_pois'): pois = rospy.get_param('~initial_pois') assert isinstance(pois, dict) for key, value in pois.iteritems(): assert type(key) == str assert type(value) == list assert len(value) == 3 name = key position = numpy_to_point(value) self._add_poi(name, position) # Update clients / markers of changes from param self.update() # Create services to add / delete / move a POI self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb) self.delete_poi_server = rospy.Service('~delete', DeletePOI, self.delete_poi_cb) self.move_poi_service = rospy.Service('~move', MovePOI, self.move_poi_cb) self.save_to_param = rospy.Service("~save_to_param", Trigger, self.save_to_param_cb) def transform_position(self, ps): ''' Attempty to transform a PointStamped message into the global frame, returning the position of the transformed point or None if transform failed. ''' # If no frame, assume user wanted it in the global frame if ps.header.frame_id == "": return ps.point try: ps_tf = self.tf_buffer.transform(ps, self.global_frame, timeout=rospy.Duration(5)) return ps_tf.point except tf2_ros.TransformException as e: rospy.logwarn('Error transforming "{}" to "{}": {}'.format( ps.header.frame_id, self.global_frame, e)) return None def process_feedback(self, feedback): ''' Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ ''' # Only look at changes when mouse button is unclicked if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP: return # Transform new position into global frame ps = PointStamped() ps.header = feedback.header ps.point = feedback.pose.position position = self.transform_position(ps) if position is None: return # Update position of marker self.update_position(feedback.marker_name, feedback.pose.position) @thread_lock(lock) def save_to_param_cb(self, req): rospy.set_param('~global_frame', self.global_frame) d = {} for poi in self.pois.pois: d[poi.name] = [ float(poi.position.x), float(poi.position.y), float(poi.position.z) ] rospy.set_param('~initial_pois', d) return {'success': True} def add_poi_cb(self, req): ''' Callback for AddPOI service ''' name = req.name # If frame is empty, just initialize it to zero if len(req.position.header.frame_id) == 0: position = numpy_to_point([0., 0., 0.]) # Otherwise transform position into global frame else: position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} if not self.add_poi(name, position): return {'success': False, 'message': 'alread exists (bad poi)'} return {'success': True, 'message': 'good poi'} def delete_poi_cb(self, req): ''' Callback for DeletePOI service ''' # Return error if marker did not exist if not self.remove_poi(req.name): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} def move_poi_cb(self, req): ''' Callback for MovePOI service ''' name = req.name # Transform position into global frame position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} # Return error if marker did not exist if not self.update_position(name, position): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} @thread_lock(lock) def add_poi(self, name, position): ''' Add a POI, updating clients / rviz when done @return False if POI already exists ''' if not self._add_poi(name, position): return False self.update() return True @thread_lock(lock) def remove_poi(self, name): ''' Remove a POI, updating clients / rviz when done @return False if POI with name does not exist ''' if not self._remove_poi(name): return False self.update() return True @thread_lock(lock) def update_position(self, name, position): ''' Update the position of a POI, updating clients / rviz when done @param position: a Point message of the new position in global frame @return False if POI with name does not exist ''' if not self._update_position(name, position): return False self.update() return True def update(self): ''' Update interactive markers server and POI publish of changes ''' self.pois.header.stamp = rospy.Time.now() self.pub.publish(self.pois) self.interactive_marker_server.applyChanges() def _update_position(self, name, position): ''' Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change ''' # Find poi with specified name for poi in self.pois.pois: if poi.name == name: pose = Pose() pose.orientation.w = 1.0 pose.position = position if not self.interactive_marker_server.setPose(name, pose): return False # Set pose in message poi.position = position return True return False def _remove_poi(self, name): ''' Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change ''' # Return false if marker with that name not added to interactive marker server if not self.interactive_marker_server.erase(name): return False # Find POI with specifed name and delete it from list for i, poi in enumerate(self.pois.pois): if poi.name == name: del self.pois.pois[i] return True return False def _add_poi(self, name, position): ''' Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change ''' if self.interactive_marker_server.get(name) is not None: return False poi = POI() poi.name = name poi.position = position self.pois.pois.append(poi) point_marker = Marker() point_marker.type = Marker.SPHERE point_marker.scale.x = self.marker_scale point_marker.scale.y = self.marker_scale point_marker.scale.z = self.marker_scale point_marker.color.r = 1.0 point_marker.color.g = 1.0 point_marker.color.b = 1.0 point_marker.color.a = 1.0 text_marker = Marker() text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.orientation.w = 1.0 text_marker.pose.position.x = 1.5 text_marker.text = poi.name text_marker.scale.z = 1.0 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 text_marker.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = self.global_frame int_marker.pose.orientation.w = 1.0 int_marker.pose.position = poi.position int_marker.scale = 1 int_marker.name = poi.name # insert a box control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.always_visible = True control.markers.append(point_marker) control.markers.append(text_marker) int_marker.controls.append(control) self.interactive_marker_server.insert(int_marker, self.process_feedback) return True
class 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 World: '''Object recognition and localization related stuff''' tf_listener = None objects = [] world = None segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service") def __init__(self): if World.tf_listener is None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') self.clear_all_objects() self.relative_frame_threshold = 0.4 # rospy.Subscriber("ar_pose_marker", # AlvarMarkers, self.receive_ar_markers) self.is_looking_for_markers = False self.marker_dims = Vector3(0.04, 0.04, 0.01) World.world = self @staticmethod def get_world(): if World.world is None: World.world = World() return World.world 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 receive_ar_markers(self, data): '''Callback function to receive marker info''' self._lock.acquire() if self.is_looking_for_markers: 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, False, id=marker.id) self._lock.release() 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, threshold=0.075): '''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 > threshold: rospy.logwarn('Found some objects, but not similar enough.') return None else: rospy.loginfo('Most similar to object ' + ref_frame_list[chosen_obj_index].name) return ref_frame_list[chosen_obj_index] @staticmethod def get_map_of_most_similar_obj(object_list, ref_frame_list, threshold=0.075): if None in object_list: object_list.remove(None) if len(object_list) == 0 or len(ref_frame_list) == 0: return None markers_dict = {} marker_names = [] for obj in object_list: object_name = obj.name if object_name.startswith("marker"): marker_names.append(object_name) found_match = False for ref_frame in ref_frame_list: if ref_frame.name == object_name: markers_dict[object_name] = ref_frame found_match = True break if not found_match: return None ref_frame_list = [x for x in ref_frame_list if x.name not in marker_names] object_list = [x for x in object_list if x.name not in marker_names] if len(object_list) == 0 or len(ref_frame_list) == 0: return markers_dict if len(markers_dict) > 0 else None orderings = [] World.permute(object_list, orderings) costs = [] assignments = [] for ordering in orderings: cur_cost = 0 cur_ref_frame_list = ref_frame_list[:] cur_assignment = [] for object in ordering: most_similar_object = World.get_most_similar_obj(object, cur_ref_frame_list, threshold) if most_similar_object is None: return None cur_ref_frame_list.remove(most_similar_object) cur_assignment.append(most_similar_object) cur_cost += World.object_dissimilarity(object, most_similar_object) costs.append(cur_cost) assignments.append(cur_assignment) min_cost, min_idx = min((val, idx) for (idx, val) in enumerate(costs)) names = [x.name for x in orderings[min_idx]] object_dict = dict(zip(names, assignments[min_idx])) object_dict.update(markers_dict) return object_dict @staticmethod def permute(a, results): if len(a) == 1: results.insert(len(results), a) else: for i in range(0, len(a)): element = a[i] a_copy = [a[j] for j in range(0, len(a)) if j != i] subresults = [] World.permute(a_copy, subresults) for subresult in subresults: result = [element] + subresult results.insert(len(results), result) @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, id=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) new_object = WorldObject(pose, n_objects, dimensions, is_recognized) if id is not None: new_object.assign_name("marker" + str(id)) new_object.is_marker = True World.objects.append(new_object) 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 if id is not None and World.objects[i].get_name() == "marker" + str(id): rospy.loginfo('Previously added marker with the same id, will not add this object.') return False n_objects = len(World.objects) new_object = WorldObject(pose, n_objects, dimensions, is_recognized) if id is not None: new_object.assign_name("marker" + str(id)) new_object.is_marker = True World.objects.append(new_object) 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 _add_new_marker(self, id, pose): '''Function to add new markers''' #dist_threshold = 0.01 #to_remove = None #for i in range(len(World.markers)): # if (World.pose_distance(World.markers[i].pose, pose) # < dist_threshold): # rospy.loginfo('Previously detected marker at the same' + # 'location, will not add this marker.') # return False #n_markers = len(World.markers) #World.markers.append(WorldMarker(id, pose)) #int_marker = self._get_object_marker(len(World.objects) - 1) #World.markers[-1].int_marker = int_marker #self._im_server.insert(int_marker, self.marker_feedback_cb) #self._im_server.applyChanges() #World.markers[-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_reachability_marker(self, world_object): radius = self.relative_frame_threshold pointsList = [] nx = 8 ny = 8 pointsList.append(Point(0, 0, radius)) pointsList.append(Point(0, 0, -radius)) for x in range(nx): theta = 2 * pi * (x * 1.0 / nx) for y in range(1, ny): phi = pi * (y * 1.0 / ny) destx = radius * cos(theta) * sin(phi) desty = radius * sin(theta) * sin(phi) destz = radius * cos(phi) pointsList.append(Point(destx, desty, destz)) id = abs(hash(world_object.get_name() + "_reachability")) % (10 ** 8) marker = Marker(type=Marker.SPHERE_LIST, id=id, lifetime=rospy.Duration(nsecs=10 ** 8), scale=Vector3(0.01, 0.01, 0.01), points=set(pointsList), header=Header(frame_id='base_link'), color=ColorRGBA(1, 1, 1, 0.5), pose=world_object.object.pose) return marker 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 color = WorldObject.default_color if not World.objects[index].is_fake else WorldObject.fake_color 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=color, pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = 0 text_pos.y = 0 text_pos.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.05, 0.05, 0.05), 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(Point(0, 0, 0), Quaternion(0, 0, 0, 1))) button_control.markers.append(object_marker) text_pos = Point() dimensions = dimensions text_pos.x = dimensions.x / 2 - 0.06 text_pos.y = - dimensions.y / 2 + 0.06 text_pos.z = dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0.05, 0.05, 0.05), 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 def get_frame_list(self): '''Function that returns the list of ref. frames''' self._lock.acquire() objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) self._lock.release() return objects @staticmethod def has_objects(): '''Function that checks if there are any objects''' return len(World.objects) > 0 @staticmethod def has_marker_objects(): '''Function that checks if there are any markers''' for o in World.objects: if o.is_marker: return True return False @staticmethod def has_non_marker_objects(): '''Function that checks if there are any objects''' for o in World.objects: if not o.is_marker: return True return False @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): 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): 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''' rospy.loginfo("waiting for segmentation service") rospy.wait_for_service(self.segmentation_service) try: get_segmentation = rospy.ServiceProxy(self.segmentation_service, TabletopSegmentation) resp = get_segmentation() rospy.loginfo("Adding landmarks") self._reset_objects() # add the table xmin = resp.table.x_min ymin = resp.table.y_min xmax = resp.table.x_max ymax = resp.table.y_max depth = xmax - xmin width = ymax - ymin pose = resp.table.pose.pose 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() for cluster in resp.clusters: points = cluster.points if (len(points) == 0): return Point(0, 0, 0) [minX, maxX, minY, maxY, minZ, maxZ] = [ points[0].x, points[0].x, points[0].y, points[0].y, points[0].z, points[0].z] for pt in points: minX = min(minX, pt.x) minY = min(minY, pt.y) minZ = min(minZ, pt.z) maxX = max(maxX, pt.x) maxY = max(maxY, pt.y) maxZ = max(maxZ, pt.z) self._add_new_object(Pose(Point((minX + maxX) / 2, (minY + maxY) / 2, (minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)), Point(maxX - minX, maxY - minY, maxZ - minZ), False) return True except rospy.ServiceException, e: print "Call to segmentation service failed: %s" % e return False
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 TrajectoryAnalyzer(): def __init__(self, marker_name): host = rospy.get_param("mongodb_host") port = rospy.get_param("mongodb_port") self._client = pymongo.MongoClient(host, port) self._traj = dict() self._retrieve_logs(marker_name) self._server = InteractiveMarkerServer(marker_name) def _retrieve_logs(self, marker_name): #logs = self._client.message_store.people_perception_marathon_uob.find() logs = self._client.message_store.people_perception.find() for log in logs: #print "logs: " + repr(log) #print "log keys: " + repr(log.keys()) for i, uuid in enumerate(log['uuids']): #if uuid not in ['21c75fa0-2ed9-5359-b4db-250142fe0f5d', '89c29b5f-e568-56ea-bca2-f3e59ddff3f7', '0824a8d9-cf9c-5aca-89fc-03e08c14275f']: # continue if uuid not in self._traj: t = Trajectory(uuid) t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) self._traj[uuid] = t else: t = self._traj[uuid] t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) #print "pose x,y: " + repr(t.uuid) + repr(t.pose[0]['position'][u'x']) + ", " + repr( t.pose[0]['position']['y']) #print "" #sys.exit(1) def visualize_trajectories(self, mode="all", average_length=0, longest_length=0): counter = 0 for uuid in self._traj: if len(self._traj[uuid].pose) > 1: if mode == "average": if abs(self._traj[uuid].length - average_length) \ < (average_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "longest": if abs(self._traj[uuid].length - longest_length) \ < (longest_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "shortest": if self._traj[uuid].length < 1: self.visualize_trajectory(self._traj[uuid]) counter += 1 else: self.visualize_trajectory(self._traj[uuid]) #print "uuid: " + repr(uuid) #raw_input("Press 'Enter' for the next trajectory.") #self.delete_trajectory(self._traj[uuid]) counter += 1 rospy.loginfo("Total Trajectories: " + str(len(self._traj))) rospy.loginfo("Printed trajectories: " + str(counter)) self.delete_trajectory(self._traj[uuid]) def _update_cb(self, feedback): return def visualize_trajectory(self, traj): int_marker = self.create_trajectory_marker(traj) self._server.insert(int_marker, self._update_cb) self._server.applyChanges() def delete_trajectory(self, traj): self._server.erase(traj.uuid) self._server.applyChanges() def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose # for i in range(len(traj.pose)): # print "Velocity: ", traj.vel[i] # print "X,Y: ", traj.pose[i]['position']['x'],\ # traj.pose[i]['position']['y'] # print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i]) # print traj.max_vel, traj.length line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 1 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker
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()