ROS 之 odom 里程计串口通讯stm32及解析

学习ROS也有一个月了，起初看了重德智能的一些视频，然后又看了古月老师的9讲视频（都在B站找的），边看跟着边操作，但是看完之后也忘得差不多了。现在写个里程计的程序，都扭扭捏捏下不了手。终于经过几天的软磨硬泡，写了这一段，在此仅作为记录学习的过程。如果有错误和不足，也希望大家帮忙指出，提出自己的意见，共同进步吧。

 

首先，还没有写 launch 文件，就直接从main 函数开始记录。

//接收其他节点的速度和角度
double Robot_Speed = 0;
double Robot_Angle = 0;

void cmdCallback(const geometry_msgs::Twist& msg)
{
    //以线速度的X方向的速度表示整体车行速度
    Robot_Speed = msg.linear.x;
    //以角速度的Z方向的速度表示整体车行角度
    Robot_Angle = msg.angular.z;
}

int main(int argc, char** argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "comm_bringup");
    //创建节点句柄
    ros::NodeHandle nh;

    //初始化comm_odom
    comm_odom::comm_odom car;
    if(!car.init())
        ROS_ERROR("comm odom initialized failed.");
    ROS_INFO("comm odom initialized successful.");

    ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback);

    //循环运行
    ros::Rate loop_rate(50);
    while (ros::ok())
    {
        ros::spinOnce();

        //机器人控制
        car.spinOnce(Robot_Speed, Robot_Angle);

        loop_rate.sleep();
    }

    return 0;
}

创建了一个新的 命名空间 comm_odom::comm_odom car; 这个 car 可以从头文件里面去查看具体的定义。

然后看到 car.init() 这个函数如下，就是初始化 串口的参数，这个串口用的 boost ，好像还有个用 serial 的包，不知道这两个是什么区别。暂时先跟着用的boost 的串口。代码如下，

bool comm_odom::init()
{
	//串口连接
	sp.set_option(boost::asio::serial_port::baud_rate(115200));
	sp.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
	sp.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
	sp.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
	sp.set_option(boost::asio::serial_port::character_size(8));

	ros::Time::init();
	current_time_ = ros::Time::now();
	last_time_ = ros::Time::now();

	//定义发布消息的名称
	pub_ = nh.advertise<nav_msgs::Odometry>("odom", 50);

	return true;
}

其中，先全局定义了串口的设备

//串口准备
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");

如果检测到了串口并连接上了，就进入下面的循环了。循环主要就是处理串口的一些数据了，包括串口发送速度，接收速度解析等。进入下面这个函数。

//机器人控制
        car.spinOnce(Robot_Speed, Robot_Angle);

//机器人控制 通信
bool comm_odom::spinOnce(double Robot_S, double Robot_A)
{
	//下发机器人期望速度
	writeSpeed(Robot_S, Robot_A);

	//读取机器人实际速度
	readSpeed();

	current_time_ = ros::Time::now();

	//发布TF
	geometry_msgs::TransformStamped odom_trans;
	odom_