订阅cmd_vel话题
1.设计一个节点,catkin_create_pkg serial_example roscoo rospy std_msgs
2.修改CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(serial_example)
find_package(catkin REQUIRED COMPONENTS
roscpp
serial
std_msgs
)
catkin_package(
CATKIN_DEPENDS
serial
std_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(serial_example_node src/serial_example_node.cpp)
target_link_libraries(serial_example_node
${catkin_LIBRARIES}
)
3.package.xml
1 <?xml version="1.0"?>
2 <package>
3 <name>serial_example</name>
4 <version>0.0.0</version>
5 <description>Example package for using http://wjwwood.io/serial/ library</description>
6
7 <author email="garyservin@gmail.com">Gary Servin</author>
8
9 <maintainer email="garyservin@gmail.com">Gary Servin</maintainer>
10
11 <license>BSD</license>
12
13 <buildtool_depend>catkin</buildtool_depend>
14 <build_depend>serial</build_depend>
15 <build_depend>std_msgs</build_depend>
16 <run_depend>serial</run_depend>
17 <run_depend>std_msgs</run_depend>
18
19 </package>
4.编译的时候可能出错 是连接不了库文件 需要下载包
cd ~/joey_ws/src
git clone git://github.com/wjwwood/serial.git
我们暂时命名为 my_serial_node ,在该节点中借助 ros-serial 实现串口的收发功能,并订阅话题 cmd_vel, 为了方便测试我们这里暂时订阅 /turtle1/cmd_vel 话题,详细看代码:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
|
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
serial::Serial ser;
void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){
ROS_INFO(
"I heard linear velocity: x-[%f],y-[%f],",cmd_vel.linear.x,cmd_vel.linear.y);
ROS_INFO(
"I heard angular velocity: [%f]",cmd_vel.angular.z);
std::
cout <<
"Twist Received" <<
std::
endl;
}
int main (int argc, char** argv){
ros::init(argc, argv,
"my_serial_node");
ros::NodeHandle nh;
ros::Subscriber write_sub = nh.subscribe(
"/turtle1/cmd_vel",
1000,cmd_vel_callback);
ros::Publisher read_pub = nh.advertise<std_msgs::String>(
"sensor",
1000);
try
{
ser.setPort(
"/dev/ttyUSB0");
ser.setBaudrate(
115200);
serial::Timeout to = serial::Timeout::simpleTimeout(
1000);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM(
"Unable to open port ");
return
-1;
}
if(ser.isOpen()){
ROS_INFO_STREAM(
"Serial Port initialized");
}
else{
return
-1;
}
|
可以通过以下方式简单的测试一下订阅的话题是否可行:
1
2
|
$
roscore &
$
rosrun turtlesim turtle_teleop_key
|
在另外一个 terminal 中运行上面的节点:
1
|
$
rosrun serial_example serial_example_node
|
当用手按下上下左右方向键时会看到实时的打印速度信息:
![](http://stevenshi.me/2017/07/18/control-mobile-base-by-serial/cmd_vel.jpg)
串口发送
节点订阅了 cmd_vel 话题,将速度、角速度数据提取出来之后,需要将这些信息以固定的协议格式通过串口送至移动底座,移动底座接收该命令并执行该命令。这里的协议格式可自行定义,也可以直接发送速度、角速度数据到移动底座;本次开发定义格式如下:
head | head | A-speed | B-speed | C-speed | CRC |
---|
0xff | 0xfe | float | float | float | u8 |
因此串口发送的总字节数是15字节,在获取到cmd_vel话题后提取有效数据并打包送至底层,串口发送借助 ros-serial 的 write 函数功能即可。
1
|
serial.
write(
buffer,buffersize)
|
串口接收与发布 odometry 话题
设计本节点还需要通过串口从移动底座获取里程计信息,并发布该里程计信息,为 Navigation stack 提供必要的 odometry 信息。串口接收的数据格式定义如下:
head | head | x-position | y-position | x-speed | y-speed | angular-speed | pose-angular | CRC |
---|
0xff | 0xae | float | float | float | float | float | float | u8 |
因此串口接收数据总字节数为27字节,串口接收完成后通过校验分析数据无误后,提取对应数据填充到 odometry 消息中,并发布出去。串口接收 ros-serial 提供了很多接口函数,因为底层发送的都是16进制格式,因此数据接收我选择了 Serial::read (uint8_t *buffer, size_t size) 函数,这个可以从 ros-serial 的源码中查到。里程计消息的发布可以参考站内文章: ROS初级十五 发布里程计消息
运动学解析函数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
|
* 运动学解析函数 将获取的 x y 方向的线速度以及角速度转变成
* 移动底座三个轮子的速度,并通过串口发送出去
* VA = Vx + L*W
* VB = -Vx*cosθ + Vy*sinθ + L*W
* VC = -Vx*cosθ - Vy*sinθ + L*W
* θ 为轮子与移动底座坐标系X轴的夹角,此处为60°
* VA VB VC 分别为三个轮子的速度
* Vx Vy 为移动底座在x y 方向的速度
* W 为移动底座绕Z轴的角速度
* L 为三个轮子到移动底座中心的距离 此处为0.15m
* ********************************************************/
void kinematics_analysis(const geometry_msgs::
Twist& cmd_vel){
float_union
VA,
VB,
VC;
VA.fvalue = cmd_vel.linear.x +
0.15*cmd_vel.angular.z;
VB.fvalue = -cmd_vel.linear.x *
0.5 + cmd_vel.linear.y *
0.867 +
0.15*cmd_vel.angular.z;
VC.fvalue = -cmd_vel.linear.x *
0.5 - cmd_vel.linear.y *
0.867 +
0.15*cmd_vel.angular.z;
memset(s_buffer,
0,sizeof(s_buffer));
//数据打包
s_buffer[
0] =
0xff;
s_buffer[
1] =
0xfe;
//
VA
s_buffer[
2] =
VA.cvalue[
0];
s_buffer[
3] =
VA.cvalue[
1];
s_buffer[
4] =
VA.cvalue[
2];
s_buffer[
5] =
VA.cvalue[
3];
//
VB
s_buffer[
6] =
VB.cvalue[
0];
s_buffer[
7] =
VB.cvalue[
1];
s_buffer[
8] =
VB.cvalue[
2];
s_buffer[
9] =
VB.cvalue[
3];
//
VC
s_buffer[
10] =
VC.cvalue[
0];
s_buffer[
11] =
VC.cvalue[
1];
s_buffer[
12] =
VC.cvalue[
2];
s_buffer[
13] =
VC.cvalue[
3];
//
CRC
s_buffer[
14] = s_buffer[
2]^s_buffer[
3]^s_buffer[
4]^s_buffer[
5]^s_buffer[
6]^s_buffer[
7]^
s_buffer[
8]^s_buffer[
9]^s_buffer[
10]^s_buffer[
11]^s_buffer[
12]^s_buffer[
13];
ser.write(s_buffer,sBUFFERSIZE);
}
|
测试
在与底盘实际通信前我们先用两台电脑进行串口的通信测试,并查看发布的 odometry 消息。一台电脑跑ROS并通过串口与另一台电脑连接,串口监控软件送出一组模拟数据:
1
2
3
4
|
0xff
0xae
0x01
0x02
0x03
0x04
0x05
0x06
0x07
0x08
0x09
0x00
0x01
0x02
0x03
0x04
0x05
0x06
0x07
0x08
0x09
0x00
0x12
0x13
0x14
0x15
0x00
|
运行节点:
1
2
|
$
roscore &
$
rosrun my_serial_node my_serial_node
|
通过串口监控软件不停的发送上述数据帧,并通过以下命令查看话题:
可以看到串口接收到底层送来的数据并实时的进行发布:
![](http://stevenshi.me/2017/07/18/control-mobile-base-by-serial/odom.jpg)