如何用Python创建ROS2里程计发布节点（含nav_msgs/Odometry与TF发布）

1. 依赖准备 #

• 确保安装对应ROS2版本的核心依赖包：

  sudo apt install ros-<your-ros2-distro>-nav-msgs ros-<your-ros2-distro>-tf2-ros ros-<your-ros2-distro>-geometry-msgs
  

• 在你的ROS2包的package.xml中声明这些依赖，同时在setup.py里注册节点（部署步骤会提到）。

2. 完整代码实现 #

以下是可直接运行的节点代码，包含里程计话题发布和TF变换广播逻辑：

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import tf_transformations

class OdomPublisher(Node):
    def __init__(self):
        super().__init__('odom_publisher')
        # 初始化里程计话题发布器（队列长度10）
        self.odom_pub = self.create_publisher(Odometry, 'odom', 10)
        # 初始化TF变换广播器
        self.tf_broadcaster = TransformBroadcaster(self)
        
        # 模拟里程计状态（实际使用时替换为传感器/运动学计算数据）
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        self.vx = 0.1  # x方向线速度
        self.vy = 0.0  # y方向线速度
        self.vtheta = 0.1  # 角速度
        
        # 10Hz定时发布数据
        self.timer = self.create_timer(0.1, self.timer_callback)

    def timer_callback(self):
        # 更新模拟的位姿数据（实际场景替换为传感器输入）
        dt = 0.1
        self.x += self.vx * dt
        self.y += self.vy * dt
        self.theta += self.vtheta * dt

        # 构造base_link到odom的TF变换消息
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'odom'
        t.child_frame_id = 'base_link'
        # 设置平移量
        t.transform.translation.x = self.x
        t.transform.translation.y = self.y
        t.transform.translation.z = 0.0
        # 欧拉角转四元数（ROS标准姿态表示）
        q = tf_transformations.quaternion_from_euler(0, 0, self.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # 构造里程计消息
        odom = Odometry()
        odom.header.stamp = t.header.stamp  # 保持时间戳同步
        odom.header.frame_id = 'odom'
        odom.child_frame_id = 'base_link'
        # 填充位姿数据
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        # 填充速度数据
        odom.twist.twist.linear.x = self.vx
        odom.twist.twist.linear.y = self.vy
        odom.twist.twist.angular.z = self.vtheta

        # 发布TF变换和里程计消息
        self.tf_broadcaster.sendTransform(t)
        self.odom_pub.publish(odom)

def main(args=None):
    rclpy.init(args=args)
    odom_publisher = OdomPublisher()
    rclpy.spin(odom_publisher)
    odom_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3. 关键逻辑解释 #

• TF变换：指定父坐标系为odom，子坐标系为base_link，使用tf_transformations库完成欧拉角到四元数的转换，这是ROS中姿态表示的标准要求。
• 里程计消息：必须保证与TF变换的时间戳一致，避免数据不同步；位姿和速度字段可直接替换为实际传感器（如编码器、IMU）或运动学模型计算的数据。
• 定时器回调：通过固定频率的定时器触发数据更新与发布，频率可根据实际需求调整（示例为10Hz）。

4. 节点部署步骤 #

1. 将代码保存为odom_publisher.py，放在你的ROS2包的scripts目录下，并添加可执行权限：

   chmod +x scripts/odom_publisher.py
   

2. 在包的setup.py中添加节点注册：

   entry_points={
       'console_scripts': [
           'odom_publisher = your_package_name.scripts.odom_publisher:main',
       ],
   },
   

3. 编译ROS2包：

   colcon build --packages-select your_package_name
   

4. 运行节点：

   ros2 run your_package_name odom_publisher
   

5. 验证方法 #

• 查看里程计话题数据：

  ros2 topic echo /odom
  

• 查看TF变换关系：

  ros2 run tf2_ros tf2_echo odom base_link
  

内容的提问来源于stack exchange，提问作者Sachmk2