python类PoseArray()的实例源码

def convertMsgToPoseArray(time, id_list, detection_msg):
    # return each robot's PoseArray

    # instatinate PoseMaker
    robot_poses = {}
    for i in id_list:
        robot_poses[i] = PoseMaker(time, 'map')

    # change detection_msg to PoseArray
    for robot in detection_msg:
        if robot.robot_id in id_list:
            robot_poses[robot.robot_id].add(robot)

    # align PoseArrays
    pose_arrays = {}
    for i in id_list:
        pose_arrays[i] = robot_poses[i].get()

    return pose_arrays

def __init__(self):
        State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints'])
        # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc.
        self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1)

        # Start thread to listen for reset messages to clear the waypoint queue
        def wait_for_path_reset():
            """thread worker function"""
            global waypoints
            while not rospy.is_shutdown():
                data = rospy.wait_for_message('/path_reset', Empty)
                rospy.loginfo('Recieved path RESET message')
                self.initialize_path_queue()
                rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches
                               # for three seconds and wait_for_message() in a
                               # loop will see it again.
        reset_thread = threading.Thread(target=wait_for_path_reset)
        reset_thread.start()

def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """

        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

def __init__(self, team_side, topic_name):
        if team_side == 'LEFT':
            self.side_coeff = 1
        else:
            self.side_coeff = -1

        self.pub = rospy.Publisher(topic_name, PoseArray, queue_size=10)

def __init__(self, time, frame_id):
        super(PoseMaker, self).__init__()
        self.pose_array = PoseArray()
        self.pose_array.header.stamp = time
        self.pose_array.header.frame_id = frame_id

def convert_PoseWithCovArray_to_PoseArray(waypoints):
    """Used to publish waypoints as pose array so that you can see them in rviz, etc."""
    poses = PoseArray()
    poses.header.frame_id = 'map'
    poses.poses = [pose.pose.pose for pose in waypoints]
    return poses

def __init__(self):
        self.sub = rospy.Subscriber('/finger_sensor_test/blockpose', PoseArray, self.update)
        self.pose = None

def publish_particles(self, particles):
        # publish the given particles as a PoseArray object
        pa = PoseArray()
        pa.header = Utils.make_header("map")
        pa.poses = Utils.particles_to_poses(particles)
        self.particle_pub.publish(pa)

def __init__(self, frequency):

        self.tag_names = rospy.get_param("tag_names")
        self.frame_id = rospy.get_param("~frame_id")
        self.transforms = rospy.get_param("tags")
        self.rate = rospy.Rate(frequency)
        #self.sub = rospy.Subscriber(self.tag_position_topic, PoseArray, self.tag_position_cb)

        self.br = tf.TransformBroadcaster()

        self.make_transforms()

def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """
        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

def _publish_places(self, places):
        """
        Publish places as poses, using a PoseArray message
        """

        if self._places_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for place in places:
                msg.poses.append(place.place_pose.pose)

            self._places_pub.publish(msg)

def publish_particles(self, particles):
        pa = PoseArray()
        pa.header = Utils.make_header("map")
        pa.poses = Utils.particles_to_poses(particles)
        self.particle_pub.publish(pa)

def _publish_places(self, places):
        """
        Publish places as poses, using a PoseArray message
        """

        if self._places_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for place in places:
                msg.poses.append(place.place_pose.pose)

            self._places_pub.publish(msg)