ROS小车wheeltec robot节点代码阅读笔记

头文件解析

#include "wheeltec_robot.h"
#include "Quaternion_Solution.h"

#include "wheeltec_robot.h"里面包括了ros和stm32之间通讯相关的函数。
#include "Quaternion_Solution.h"里面主要包含了四元素的转换

主函数解析

int main(int argc, char** argv)
{
  ros::init(argc, argv, "wheeltec_robot"); 
  turn_on_robot Robot_Control; 
  Robot_Control.Control()
  return 0;  
} 

1、ros::init(argc, argv, "wheeltec_robot");ROS的初始化，必须有的，并且将该节点叫做wheeltec_robot。

2、turn_on_robot Robot_Control：将turn_on_robot类创建Robot_control对象并自动调用构造函数初始化，也就是在#include "wheeltec_robot.h"里面有个类叫turn_on_robot，我们把该类称为Robot_Control

3、Robot_Control.Control();是一个回调函数，用于循环执行数据采集和发布话题等操作，所以我们可以先看这个回调函数的内容。

void turn_on_robot::Control()
{
  _Last_Time = ros::Time::now();
  while(ros::ok())
  {
    _Now = ros::Time::now();
    Sampling_Time = (_Now - _Last_Time).toSec();

    if (true == Get_Sensor_Data()) 
    {
      Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; 
      Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;
      Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;
      Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
                Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);

      Publish_Odom();   
      Publish_ImuSensor();
      Publish_Voltage(); 
    }
    _Last_Time = _Now;
    ros::spinOnce(); 
    }
}

 _Now = ros::Time::now()：获取当前时间

 Sampling_Time = (_Now - _Last_Time).toSec()：Sampling_Time在wheeltec_robot.h中已经定义了，此段代码主要是获取时间间隔，以便后面可以计算积分速度获取里程。

最主要还是要看到下面三行代码：

      Publish_Odom();
      Publish_ImuSensor();
      Publish_Voltage();

说明又有三个回调函数，我们一个一个的去看：

Publish_Odom()

发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵

void turn_on_robot::Publish_Odom()
{
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);

    nav_msgs::Odometry odom; 
    odom.header.stamp = ros::Time::now(); 
    odom.header.frame_id = odom_frame_id; 
    odom.pose.pose.position.x = Robot_Pos.X; 
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = Robot_Pos.Z;
    odom.pose.pose.orientation = odom_quat; 
    odom.child_frame_id = robot_frame_id; 
    odom.twist.twist.linear.x =  Robot_Vel.X; 
    odom.twist.twist.linear.y =  Robot_Vel.Y; 
    odom.twist.twist.angular.z = Robot_Vel.Z; 
    if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0)
      memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
      memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    else
      memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
      memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       
    odom_publisher.publish(odom);
}

geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z):把把Z轴转角转换为四元数进行表达

nav_msgs::Odometry odom：nav_msgs是ros的内置函数，nav_msgs::Odometry里面主要是关注header.stamp、header.frame_id、pose.position.x、pose.position.y、pose.position.z、child_frame_id等参数

然后下面有个if_else的函数，其主要功能就是卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包。

Publish_ImuSensor()

void turn_on_robot::Publish_ImuSensor()
{
  sensor_msgs::Imu Imu_Data_Pub;
  Imu_Data_Pub.header.stamp = ros::Time::now(); 
  Imu_Data_Pub.header.frame_id = gyro_frame_id; 
  Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
  Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; 
  Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
  Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
  Imu_Data_Pub.orientation_covariance[0] = 1e6; 
  Imu_Data_Pub.orientation_covariance[4] = 1e6;
  Imu_Data_Pub.orientation_covariance[8] = 1e-6;
  Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; /
  Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
  Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
  Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; 
  Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
  Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
  Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
  Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; 
  Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;  
  imu_publisher.publish(Imu_Data_Pub);
}

主要的功能是发布IMU的数据话题，此IMU用的是Mpu6050型号

 Imu_Data_Pub.orientation.x = Mpu6050.orientation.x获取Mpu6050的姿态

  Imu_Data_Pub.orientation_covariance[0] = 1e6;
  Imu_Data_Pub.orientation_covariance[4] = 1e6;
  Imu_Data_Pub.orientation_covariance[8] = 1e-6;

四元素表达为三轴姿态，其矩阵在wheeltec_robot.h中有。