sensor_imu_publisher.py 文件源码

python
阅读 19 收藏 0 点赞 0 评论 0

项目:autonomous_bicycle 作者: SiChiTong 项目源码 文件源码
def __init__(self):
        self.degrees2rad = math.pi/180.0

        # Get values from launch file
        self.rate = rospy.get_param('~rate', 80.0)  # the rate at which to publish the transform
        # Static transform between sensor and fixed frame: x, y, z, roll, pitch, yaw
        # <rosparam param="static_transform">[0, 0, 0, 0, 0, 0]</rosparam>
        self.static_transform = rospy.get_param('~static_transform', [0, 0, 0, 0, 0, 0])
        self.serial_port = rospy.get_param('~serial_port', "/dev/ttyUSB0")
        self.topic_name = rospy.get_param('~topic_name', "/imu")
        self.fixed_frame = rospy.get_param('~fixed_frame', "world")
        self.frame_name = rospy.get_param('~frame_name', "imu")
        self.publish_transform = rospy.get_param('~publish_transform', False)

        self.imu = ImuDriver(serial_port=self.serial_port)
        self.imu.init_imu()

        # Create a publisher for imu message
        self.pub_imu = rospy.Publisher(self.topic_name, Imu, queue_size=1)
        self.odomBroadcaster_imu = TransformBroadcaster()

        self.imu_msg = Imu()
        self.imu_msg.orientation_covariance[0] = -1
        self.imu_msg.angular_velocity_covariance[0] = -1
        self.imu_msg.linear_acceleration_covariance[0] = -1

        self.current_time = rospy.get_time()
        self.last_time = rospy.get_time()

        self.seq = 0

        rospy.on_shutdown(self.shutdown_node)
        rate = rospy.Rate(self.rate)

        rospy.loginfo("Ready for publishing imu:" + self.serial_port)

        # Main while loop.
        while not rospy.is_shutdown():
            self.current_time = rospy.get_time()

            self.imu.read()

            if self.publish_transform:
                quaternion = self.imu.quaternion_from_euler(self.static_transform[3]*self.degrees2rad,
                                                            self.static_transform[4]*self.degrees2rad,
                                                            self.static_transform[5]*self.degrees2rad)

                # send static transformation tf between imu and fixed frame
                self.odomBroadcaster_imu.sendTransform(
                    (self.static_transform[0], self.static_transform[1], self.static_transform[2]),
                    (quaternion[0], quaternion[1], quaternion[2], quaternion[3]),
                    rospy.Time.now(), self.frame_name, self.fixed_frame
                )

            # publish imu message
            self.publish_info(imu=self.imu)
            rate.sleep()
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号