ROS2代码阅读笔记1-发布里程计消息和广播相应的 TF 变换

#include <geometry_msgs/msg/transform_stamped.hpp>
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>
#include <string>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

#include <memory>
#include <string>

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using std::placeholders::_1;

class OdomPublisher:public rclcpp ::Node
{
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   double linear_scale_x_ = 0.0 ;
   double linear_scale_y_ = 0.0;
   double vel_dt_ = 0.0;
   double x_pos_ = 0.0;
   double y_pos_ = 0.0;
   double heading_ = 0.0;
   double linear_velocity_x_ = 0.0;
   double linear_velocity_y_ = 0.0;
   double angular_velocity_z_ = 0.0;
   double wheelbase_ = 0.25;
   bool pub_odom_tf_ = false;
   rclcpp::Time last_vel_time_  ;
	public:
	  OdomPublisher()
	  : Node("base_node")
	  {            
            this->declare_parameter<double>("wheelbase",0.25);
            this->declare_parameter<std::string>("odom_frame","odom");
            this->declare_parameter<std::string>("base_footprint_frame","base_footprint"); 
            this->declare_parameter<double>("linear_scale_x",1.0);
            this->declare_parameter<double>("linear_scale_y",1.0);
            this->declare_parameter<bool>("pub_odom_tf",true);

            this->get_parameter<double>("linear_scale_x",linear_scale_x_);
            this->get_parameter<double>("linear_scale_y",linear_scale_y_);
            this->get_parameter<double>("wheelbase",wheelbase_);
            this->get_parameter<bool>("pub_odom_tf",pub_odom_tf_);
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this); //?

	  	subscription_ = this->create_subscription<geometry_msgs::msg::Twist>("vel_raw",50,std::bind(&OdomPublisher::handle_vel,this,_1));
	  	odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("/odom_raw", 50);
	  	}
	  	private:
	  	  void handle_vel(const std::shared_ptr<geometry_msgs::msg::Twist > msg)
	  	  {
            //geometry_msgs::msg::Twist twist;
                       
	  	  	rclcpp::Time curren_time = rclcpp::Clock().now();
	  	  	linear_velocity_x_ = msg->linear.x * linear_scale_x_;// scale = 1
    		linear_velocity_y_ = msg->linear.y * linear_scale_y_;
    		angular_velocity_z_ = msg->angular.z ;
			vel_dt_ = (curren_time - last_vel_time_).seconds();
            //std::cout<<"curren_time: "<<curren_time.seconds()<<std::endl;
            // std::cout<<"vel_dt: "<<vel_dt_<<std::endl;
    		last_vel_time_ = curren_time;
			double steer_angle = linear_velocity_y_;
			double MI_PI = 3.1416;
			
    		double delta_heading = angular_velocity_z_ * vel_dt_; //radians
    		double delta_x = (linear_velocity_x_ * cos(heading_)-linear_velocity_y_*sin(heading_)) * vel_dt_; //m
    		double delta_y = (linear_velocity_x_ * sin(heading_)+linear_velocity_y_*cos(heading_)) * vel_dt_; //m	
 			x_pos_ += delta_x;
    		y_pos_ += delta_y;
			heading_ += delta_heading;
            
            tf2::Quaternion myQuaternion;
			geometry_msgs::msg::Quaternion odom_quat ; 
			myQuaternion.setRPY(0.00,0.00,heading_ );

            odom_quat.x = myQuaternion.x();
            odom_quat.y = myQuaternion.y();
            odom_quat.z = myQuaternion.z();
            odom_quat.w = myQuaternion.w();
  
			nav_msgs::msg::Odometry odom;
			odom.header.stamp = curren_time;
			odom.header.frame_id = "odom";
			odom.child_frame_id = "base_footprint";
			// robot's position in x,y and z
			odom.pose.pose.position.x = x_pos_;
			odom.pose.pose.position.y = y_pos_;
			odom.pose.pose.position.z = 0.0;
			// robot's heading in quaternion

            odom.pose.pose.orientation = odom_quat;
			odom.pose.covariance[0] = 0.001;
			odom.pose.covariance[7] = 0.001;
			odom.pose.covariance[35] = 0.001;
			// linear speed from encoders
			odom.twist.twist.linear.x = linear_velocity_x_;
			odom.twist.twist.linear.y = linear_velocity_y_;
			odom.twist.twist.linear.y = 0.0; // vy = 0.0
			odom.twist.twist.linear.z = 0.0;
			odom.twist.twist.angular.x = 0.0;
			odom.twist.twist.angular.y = 0.0;
			// angular speed from encoders
			odom.twist.twist.angular.z = angular_velocity_z_;
			odom.twist.covariance[0] = 0.0001;
			odom.twist.covariance[7] = 0.0001;
			odom.twist.covariance[35] = 0.0001;
			// ROS_INFO("ODOM PUBLISH");	
			odom_publisher_ -> publish(odom);
            if (pub_odom_tf_)
            {
            
                geometry_msgs::msg::TransformStamped t;
                rclcpp::Time now = this->get_clock()->now();
                t.header.stamp = now;
                t.header.frame_id = "odom";
                t.child_frame_id = "base_footprint";
                t.transform.translation.x = x_pos_;
                t.transform.translation.y = y_pos_;
                t.transform.translation.z = 0.0;
                
                t.transform.rotation.x = myQuaternion.x();
                t.transform.rotation.y = myQuaternion.y();
                t.transform.rotation.z = myQuaternion.z();
                t.transform.rotation.w = myQuaternion.w();
                
                tf_broadcaster_->sendTransform(t); //?
                  
            }
		  	  }

};


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

1.include <tf2_ros/transform_broadcaster.h>

• 主要功能

• tf2_ros::TransformBroadcaster 用于将坐标系的变换（即从一个坐标系到另一个坐标系的姿态变化）发布到 ROS2 的 tf 框架。这个框架是机器人操作系统（ROS）的一部分，专门用于管理不同坐标系之间的变换，并提供将这些变换广播给其他节点的机制。

• 主要用途

• 广播机器人状态: 发布机器人的位姿变换，如从底盘坐标系（base_link）到世界坐标系（map）的变换。
• 传感器数据处理: 将传感器数据的坐标系转换为机器人坐标系或世界坐标系，方便其他节点使用。

• 运动规划和导航: 在机器人导航中，tf2_ros::TransformBroadcaster 用于广播机器人的当前位置和方向，从而帮助导航算法进行路径规划。

• std::make_unique 是 C++11 中引入的一种创建智能指针的方法。它返回一个 std::unique_ptr，该指针管理动态分配的对象。使用 std::make_unique 可以避免手动使用 new 操作符，从而减少内存泄漏的风险

2.nav_msgs

• nav_msgs 是 ROS（Robot Operating System）中的一个消息包，包含了与导航相关的消息类型。这些消息类型用于表示机器人导航过程中常见的数据结构，如路径、里程计、地图等。

• 以下是 nav_msgs 包中常用的一些消息类型：

• nav_msgs/Odometry

• 用于表示机器人在空间中的位置、速度和姿态（orientation）。
• 包含以下字段：
• header: 消息头，包括时间戳和坐标系。
• child_frame_id: 子坐标系ID，一般是机器人底盘坐标系。
• pose: 包含位置和姿态的位置信息。
• twist: 包含线速度和角速度的速度信息。

•  nav_msgs/Path

• 用于表示机器人计划路径或已经行驶的路径。
• 包含以下字段：
• header: 消息头，包括时间戳和坐标系。
• poses: 一系列姿态点，表示路径上的各个点

• nav_msgs/OccupancyGrid

• 用于表示栅格地图，常用于环境建模和路径规划。
• 包含以下字段：
• header: 消息头，包括时间戳和坐标系。
• info: 地图信息，包括分辨率、宽度、高度等。
• data: 栅格数据，每个栅格的占用情况。

• nav_msgs/GetMap

• 用于获取地图的服务消息类型。

3.tf2::Quaternion myQuaternion;解释

• tf2::Quaternion myQuaternion; 是用来创建一个 tf2 库中的四元数对象。四元数是一种用于表示和计算三维旋转的数学工具。相比于欧拉角，四元数避免了万向锁（gimbal lock）问题，且在插值计算（如旋转平滑过渡）中更具优势。

 4.geometry_msgs::msg::Quaternion odom_quat ; 解释

•  geometry_msgs::msg::Quaternion odom_quat; 是在 ROS2 中声明一个四元数消息对象。这个对象属于 geometry_msgs 包中的 Quaternion 消息类型，用于表示四元数，即一种描述旋转的数学表示法。

 5.geometry_msgs::msg::TransformStamped t;解释

 

• 定义了一个名为 t 的geometry_msgs::msg::TransformStamped 类型的变量。这个变量用于表示一个坐标变换（transform），具体包括了坐标系之间的相对位置信息（平移和旋转），以及相关的时间戳和坐标系标识符。下面是对这个变量及其主要成员的详细解释：

• geometry_msgs::msg::TransformStamped 是 ROS 2 中的一种消息类型，用于描述两个坐标系之间的变换。它包含以下主要成员：

• header:

  • stamp (时间戳): 记录变换的时间。
  • frame_id (坐标系ID): 变换的参考坐标系。

• child_frame_id:

  • 子坐标系的ID，即被变换的坐标系。

• transform:

  • translation (平移): 表示从参考坐标系到子坐标系的平移向量。
  • rotation (旋转): 表示从参考坐标系到子坐标系的旋转（使用四元数表示）。

 

 6.tf_broadcaster_->sendTransform(t); 解释

 

• tf_broadcaster_->sendTransform(t); 这行代码用于发布一个坐标变换，使其他 ROS 2 节点可以获取这个变换信息。具体来说，它将变量 t 中描述的坐标系之间的变换发送出去。以下是对其详细解释：

• sendTransform 函数

• tf_broadcaster_->sendTransform(t); 调用 tf_broadcaster_ 对象的 sendTransform 方法，该方法将一个 geometry_msgs::msg::TransformStamped 类型的变换消息发布到 ROS 2 的 TF 框架中。

• TF 框架用于跟踪多个坐标系之间的关系，并维护它们在随时间变化时的变换信息。sendTransform 方法将新的变换信息发布到 TF 框架，使其他订阅这个变换信息的节点能够接收到并使用这些信息。