base_control.cpp代码如下：
/******************************************************************
基于串口通信的ROS小车基础控制器，功能如下：
1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动
2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动
3.发布里程计主题/odm 串口通信说明：
1.写入串口 （1）内容：左右轮速度，单位为mm/s （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]
2.读取串口 （1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
*******************************************************************/
#include "ros/ros.h" //导入ROS系统包含核心公共头文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>//我们需要实现“odom”参考系到“base_link”参考系的变换
#include <nav_msgs/Odometry.h>//以及nav_msgs/Odometry消息的发布
//以下为串口通讯需要的头文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "serial/serial.h"
/****************************************************************************/
using std::string; //使用C＋＋标准程序库的任何标识符
using std::exception; //还可以有以下两种写法
using std::cout; //直接指定标识符。例如std::ostream而不是ostream
using std::cerr; //如std::cout << std::hex << 3.4 << std::endl;
using std::endl; //最方便的就是使用using namespace std;这样命名空间std内定义的所有标识符都有效（曝光）。就好像它们被声明为全局变量一样
using std::vector; //#include <iostream>，#include <sstream>，#include <string>，using namespace std;
/*****************************************************************************/
float ratio = 1000.0f ; //转速转换比例，执行速度调整比例
float D = 0.2680859f ; //两轮间距，单位是m
float linear_temp=0,angular_temp=0;//暂存的线速度和角速度
/****************************************************/
unsigned char data_terminal0=0x0d; //“/r"字符回车
unsigned char data_terminal1=0x0a; //“/n"字符换行
// 0d 0a这是自行定义的一种数据帧结束标志
unsigned char speed_data[10]={0}; //要发给串口的数据
string rec_buffer; //串口数据接收变量
//String：适用于少量的字符串操作的情况
//StringBuilder：适用于单线程下在字符缓冲区进行大量操作的情况
//StringBuffer：适用多线程下在字符缓冲区进行大量操作的情况
//发送给下位机的左右轮速度，里程计的坐标和方向
union floatData //union的作用为实现char数组和float之间的转换
{
float d;
unsigned char data[4];
}right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
//Twist消息，它的Topic是/cmd_vel，base controler订阅Twist消息来控制电机。
{
string port("/dev/ttyUSB0"); //小车串口号
unsigned long baud = 115200; //小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
//将转换好的小车速度分量为左右轮速度
left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;
//存入数据到要发布的左右轮速度消息
left_speed_data.d*=ratio; //放大１０００倍，mm/s
right_speed_data.d*=ratio;//放大１０００倍，mm/s
for(int i=0;i<4;i++) //将左右轮速度存入数组中发送给串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
int main(int argc, char **argv)// argv 是一个指针数组，他的元素个数是argc，存放的是指向每一个参数的指针
{
string port("/dev/ttyUSB0");//小车串口号
unsigned long baud = 115200;//小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口
ros::init(argc, argv, "base_controller");//初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20);
//发布一个消息类型为nav_msgs，命名为odom的话题。定义消息队列大小为20，即超过20条消息之后，旧的消息就会丢弃
static tf::TransformBroadcaster odom_broadcaster;//定义一个tf广播，来发布tf变换信息。
geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
nav_msgs::Odometry odom;//定义里程计对象
geometry_msgs::Quaternion odom_quat; //四元数变量
//定义covariance协方差矩阵，作用为解决文职和速度的不同测量的不确定性
float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x
0, 0.01, 0, 0, 0, 0, // covariance on gps_y
0, 0, 99999, 0, 0, 0, // covariance on gps_z
0, 0, 0, 99999, 0, 0, // large covariance on rot x
0, 0, 0, 0, 99999, 0, // large covariance on rot y
0, 0, 0, 0, 0, 0.01}; // large covariance on rot z
//载入covariance矩阵
for(int i = 0; i < 36; i++)
{
odom.pose.covariance[i] = covariance[i];;
}
ros::Rate loop_rate(10);
//设置周期休眠时间，指定发布消息的频率，这里指10Hz，也即每秒10次
//通过 Rate::sleep()来处理睡眠的时间来控制对应的发布频率。
while(ros::ok())
//默认roscpp会植入一个SIGINT处理机制，当按下Ctrl-C，就会让ros::ok()返回false,那循环就会结束。
//ros::ok() 返回false的几种情况：
//SIGINT收到(Ctrl-C)信号
//另一个同名节点启动，会先中止之前的同名节点
//ros::shutdown()被调用
//所有的ros::NodeHandles被销毁
{
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
//readLine()是读取流读数据的时候用的，当读到换行标记'\n'、'\r'（回车）时，会跟着换行，同时会以字符串形式返回这一行的数据，当读取完所有的数据时会返回null
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息
{
for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度
{
position_x.data[i]=receive_data[i];
position_y.data[i]=receive_data[i+4];
oriention.data[i]=receive_data[i+8];
vel_linear.data[i]=receive_data[i+12];
vel_angular.data[i]=receive_data[i+16];
}
//将X，Y坐标，线速度缩小1000倍
position_x.d/=1000; //m
position_y.d/=1000; //m
vel_linear.d/=1000; //m/s
// 为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布
odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数
//载入坐标（tf）变换时间戳
//时间戳是一个时间标记，用以表明该信息的唯一性。
odom_trans.header.stamp = ros::Time::now();
//发布坐标变换的父子坐标系
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
//tf位置数据：x,y,z,方向
odom_trans.transform.translation.x = position_x.d;
odom_trans.transform.translation.y = position_y.d;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//发布tf坐标变化
odom_broadcaster.sendTransform(odom_trans);
//载入里程计时间戳
odom.header.stamp = ros::Time::now();
//里程计的父子坐标系
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//里程计位置数据：x,y,z,方向
odom.pose.pose.position.x = position_x.d;
odom.pose.pose.position.y = position_y.d;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//载入线速度和角速度
odom.twist.twist.linear.x = vel_linear.d;
//odom.twist.twist.linear.y = odom_vy;
odom.twist.twist.angular.z = vel_angular.d;
//发布里程计
odom_pub.publish(odom);
ros::spinOnce();//周期执行如果程序里也有订阅话题，就必需，否则回调函数不能起作用。
loop_rate.sleep();//周期休眠
}
//程序周期性调用
//ros::spinOnce(); //callback函数必须处理所有问题时，才可以用到
}
return 0;
}
odom：
里程计坐标系，这里要区分开odom topic，这是两个概念，一个是坐标系，一个是根据编码器（或者视觉等）计算的里程计。但是两者也有关系，odom topic 转化得位姿矩阵是odom–>base_link的tf关系。这时可有会有疑问，odom和map坐标系是不是重合的？（这也是我写这个博客解决的主要问题）可以很肯定的告诉你，机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如gmapping会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0.
quat：
oculus获取的pose数据包含坐标和四元数信息，但所处的坐标系和CryEngine坐标系不同
oculus坐标系 ：+x向右，+y向上，+z向外（屏幕外）
cryengine坐标系：+x向右，+y向内（屏幕以内），+z向上
回调函数ros::spin()与ros::spinOnce()
ros::spin()
这句话的意思是循环且监听反馈函数（callback）。循环就是指程序运行到这里，就会一直在这里循环了。监听反馈函数的意思是，如果这个节点有callback函数，那写一句ros::spin()在这里，就可以在有对应消息到来的时候，运行callback函数里面的内容。
就目前而言，这句话适用于写在程序的末尾（因为写在这句话后面的代码不会被执行），适用于订阅节点，且订阅速度没有限制的情况。
ros::spinOnce()
这句话的意思是监听反馈函数（callback）。只能监听反馈，不能循环。所以当你需要监听一下的时候，就调用一下这个函数。
这个函数比较灵活，尤其是我想控制接收速度的时候。配合ros::ok()效果极佳
ROS的主循环中需要不断调用ros::spin()或ros::spinOnce()，两者区别在于前者调用后不会再返回，而后者在调用后还可以继续执行之后的程序。
在使用ros::spin()的情况下，一般来说在初始化时已经设置好所有消息的回调，并且不需要其他背景程序运行。这样一来，每次消息到达时会执行用户的回调函数进行操作，相当于程序是消息事件驱动的；而在使用ros::spinOnce()的情况下，一般来说仅仅使用回调不足以完成任务，还需要其他辅助程序的执行：比如定时任务、数据处理、用户界面等。
原理：除了用户的主程序以外，ROS的socket连接控制进程会在后台接收订阅的消息，所有接收到的消息并不是立即处理，而是等到spin()或者spinOnce()执行时才集中处理。
I. 对于速度较快的消息，需要注意合理控制消息队列及spinOnce()的时间。例如，如果消息到达的频率是100Hz，而spinOnce()的执行频率是10Hz，那么就要至少保证消息队列中预留的大小大于10。
II. 如果对于用户自己的周期性任务，最好和spinOnce()并列调用。即使该任务是周期性的对于数据进行处理，例如对接收到的IMU数据进行Kalman滤波，也不建议直接放在回调函数中：因为存在通信接收的不确定性，不能保证该回调执行在时间上的稳定性。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
