Python InteractiveMarkerServer.erase方法代碼示例

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import erase [as 別名]
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
#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import erase [as 別名]
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()

#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import erase [as 別名]
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
#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import erase [as 別名]
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]))

#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import erase [as 別名]
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'],\
#.........這裏部分代碼省略.........