mobile_tracker.py 文件源码

def alvarcb(self, markers):
        rospy.logdebug("Detected markers!")
        # can we find the correct marker?
        for m in markers.markers:
            if m.id == self.marker_id:
                odom_meas = Odometry()
                odom_meas.header.frame_id = self.frame_id
                m.pose.header.frame_id = self.camera_frame_id
                odom_meas.child_frame_id = self.base_frame_id
                odom_meas.header.stamp = m.header.stamp
                m.pose.header.stamp = m.header.stamp
                # now we need to transform this pose measurement from the camera
                # frame into the frame that we are reporting measure odometry in
                pose_transformed = self.transform_pose(m.pose)
                if pose_transformed is not None:
                    odom_meas.pose.pose = pose_transformed.pose
                    # Now let's add our offsets:
                    odom_meas = odom_conversions.odom_add_offset(odom_meas, self.odom_offset)
                    self.meas_pub.publish(odom_meas)
                    self.send_transforms(odom_meas)
                    self.publish_path(m.pose)
        return