在ROS 2中发布nav_msgs/Odometry（里程计）消息

一、使用Python发布nav_msgs/Odometry消息

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose, Twist, Vector3
from nav_msgs.msg import Odometry
import tf2_ros
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import math

class OdometryPublisher(Node):
    def __init__(self):
        super().__init__('odometry_publisher')
        self.publisher = self.create_publisher(Odometry, 'odom', 10)
        self.timer = self.create_timer(0.1, self.publish_odometry)
        self.odom_frame_id = 'odom'
        self.child_frame_id = 'base_link'
        self.odom_broadcaster = TransformBroadcaster(self)

        # 初始化里程计变量
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        self.vx = 0.1  # 例子中使用的线速度
        self.vtheta = 0.1  # 例子中使用的角速度

    def publish_odometry(self):
        # 更新里程计信息
        self.x += self.vx * math.cos(self.theta)
        self.y += self.vx * math.sin(self.theta)
        self.theta += self.vtheta

        # 发布TransformStamped消息
        odom_trans = TransformStamped()
        odom_trans.header.stamp = self.get_clock().now().to_msg()
        odom_trans.header.frame_id = self.odom_frame_id
        odom_trans.child_frame_id = self.child_frame_id
        odom_trans.transform.translation.x = self.x
        odom_trans.transform.translation.y = self.y
        odom_trans.transform.rotation.w = math.cos(self.theta / 2)
        odom_trans.transform.rotation.z = math.sin(self.theta / 2)
        self.odom_broadcaster.sendTransform(odom_trans)

        # 发布Odometry消息
        odom_msg = Odometry()
        odom_msg.header.stamp = self.get_clock().now().to_msg()
        odom_msg.header.frame_id = self.odom_frame_id
        odom_msg.child_frame_id = self.child_frame_id
        odom_msg.pose.pose.position.x = self.x
        odom_msg.pose.pose.position.y = self.y
        odom_msg.pose.pose.orientation.w = math.cos(self.theta / 2)
        odom_msg.pose.pose.orientation.z = math.sin(self.theta / 2)
        odom_msg.twist.twist.linear.x = self.vx
        odom_msg.twist.twist.angular.z = self.vtheta

        self.publisher.publish(odom_msg)

def main(args=None):
    rclpy.init(args=args)
    node = OdometryPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

二、使用C++发布nav_msgs/Odometry消息

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

class OdometryPublisher : public rclcpp::Node {
public:
    OdometryPublisher()
        : Node("odometry_publisher"), x(0.0), y(0.0), theta(0.0), vx(0.1), vtheta(0.1) {
        publisher_ = this->create_publisher("odom", 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&OdometryPublisher::publish_odometry, this));

        odom_broadcaster_ = std::make_shared(this);
    }

private:
    void publish_odometry() {
        // 更新里程计信息
        x += vx * std::cos(theta);
        y += vx * std::sin(theta);
        theta += vtheta;

        // 发布TransformStamped消息
        geometry_msgs::msg::TransformStamped odom_trans;
        odom_trans.header.stamp = this->now();
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.rotation = tf2::toMsg(tf2::Quaternion(0, 0, std::sin(theta / 2), std::cos(theta / 2)));
        odom_broadcaster_->sendTransform(odom_trans);

        // 发布Odometry消息
        nav_msgs::msg::Odometry odom_msg;
        odom_msg.header.stamp = this->now();
        odom_msg.header.frame_id = "odom";
        odom_msg.child_frame_id = "base_link";
        odom_msg.pose.pose.position.x = x;
        odom_msg.pose.pose.position.y = y;
        odom_msg.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, std::sin(theta / 2), std::cos(theta / 2)));
        odom_msg.twist.twist.linear.x = vx;
        odom_msg.twist.twist.angular.z = vtheta;

        publisher_->publish(odom_msg);
    }

    rclcpp::Publisher::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr odom_broadcaster_;

    double x, y, theta, vx, vtheta;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared());
    rclcpp::shutdown();
    return 0;
}

tf2_ros::TransformBroadcaster的作用:

在ROS中，TF是一种用于跟踪和广播坐标系变换的机制，它允许多个节点之间共享坐标系信息。在机器人控制中，TF通常用于描述不同坐标系之间的关系，如机器人基座坐标系（base_link）相对于全局坐标系（odom）的变换。

在上述的代码中，odom_broadcaster_ 用于发布机器人的里程计信息，即机器人在全局坐标系中的位置和方向变换。具体来说，它发布了一个geometry_msgs/TransformStamped消息，描述了机器人的当前位姿。

这个变换消息包含了三个关键的信息：

• header: 包含了时间戳和坐标系的信息。
• child_frame_id: 子坐标系的名称，即机器人坐标系（base_link）。
• transform: 描述了从父坐标系到子坐标系的变换信息，包括平移和旋转。

在发布nav_msgs/Odometry消息之前，通过odom_broadcaster_发布的TF消息，可以被其他ROS节点监听和使用，以获取机器人在全局坐标系中的准确位置和方向信息。这对于在ROS系统中实现机器人导航和感知非常重要。