系列文章目录
北科天绘 16线3维激光雷达开发教程
基于Rplidar二维雷达使用Hector_SLAM算法在ROS中建图
Nvidia Jetson Nano学习笔记–串口通信
Nvidia Jetson Nano学习笔记–使用C语言实现GPIO 输入输出
Autolabor ROS机器人教程
从零搭建一台基于ROS的自动驾驶车-----1.整体介绍
前言
在整个智能车中运动控制是最基础也是最为重要的一步，本文主要的内容有：
1.ROS通过串口与STM32实现通信，继而控制智能车运动
2.在ROS中通过键盘运动控制节点来实现小车的运动
主要参考：基于ROS平台的STM32小车–汇总
一、串口通信
ros中有现成的串口功能包来通信。
1.首先创建一个ROS功能空间
mkdir -p ros_car(自定义空间名称)/src
cd roscar
catkin_make
上述命令，首先会创建一个工作空间以及一个 src 子目录，然后再进入工作空间调用 catkin_make命令编译。2.下载ROS串口通信的功能包
cd ~/ros_car/src
git clone https://github.com/ncnynl/teleop_twist_keyboard.git3.编译
cd ~/catkin_ws
catkin_make4.编写主要程序
$ cd ~/ros_car/src
$ catkin_create_pkg base_controller roscpp
$ cd ros_car/src/base_controller
$ mkdir src
$ touch src/base_controller.cpp
$ gedit src/base_controller.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>
#include <nav_msgs/Odometry.h>
//以下为串口通讯需要的头文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "serial/serial.h"
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*****************************************************************************/
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"字符
unsigned char speed_data[10]={0}; //要发给串口的数据
string rec_buffer; //串口数据接收变量//发送给下位机的左右轮速度，里程计的坐标和方向
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主题回调函数
{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/slinear_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/sright_speed_data.d*=ratio;//放大１０００倍，mm/sfor(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)
{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); //定义要发布/odom主题static tf::TransformBroadcaster odom_broadcaster;//定义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_x0, 0.01, 0, 0, 0, 0, // covariance on gps_y0, 0, 99999, 0, 0, 0, // covariance on gps_z0, 0, 0, 99999, 0, 0, // large covariance on rot x0, 0, 0, 0, 99999, 0, // large covariance on rot y0, 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);//设置周期休眠时间while(ros::ok()){/*rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据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; //mposition_y.d/=1000; //mvel_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;
}在上面的代码中主要是订阅了cmd_vel这个话题，将cmd_vel这个话题的数据就是控制小车运动的数据，将其解算一下，通过my_serial.write(speed_data,10); 将数据发送给串口来控制小车运动，注释掉的代码其作用是采集串口反馈回来的小车的速度，通过odom这个话题发送出去，但是在目前是用不到这段代码的。
主要用到了X轴的线速度和Z轴的角速度。
5.修改 CMakeLists.txt
find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_generationserialtfnav_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES base_controllerCATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(${catkin_INCLUDE_DIRS}${serial_INCLUDE_DIRS}
)
add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES})编译完，运行这个ros节点，该节点会订阅cmd_vel这个话题， 继而控制小车运动。
需要注意的是设备的串口号和波特率需要根据实际情况进行修改。
二、键盘控制
键盘控制节点只需要发送cmd_vel这个话题给base_controller这个节点就可以实现键盘控制小车运动。
1.下载键盘控制的ROS包
cd ~/ros_car/src
git clone https://github.com/ncnynl/teleop_twist_keyboard.git2.启动键盘控制节点
$cd ~/ros_car
$catkin_make
$ roscore
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
$ rosrun base_controller base_controller
