基于ROS和beaglebone的串口通信方式，使用键盘控制移动机器人

一、所需工具包

1.ROS键盘包：teleop_twist_keyboard 
 2.ROS串口通讯包：serial

1. $ cd ~/catkin_ws/src
2. $ git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git
3. $ git clone https://github.com/Forrest-Z/serial.git
4. $ catkin_make

3.在ubuntu的ros中建立一个ros_car_pkg包:

1. $ cd ~/catkin_ws/src
2. $ catkin_create_pkg ros_car_pkg roscpp rospy std_msgs

4.新建 base_controller 文件:

1. $ cd catkin_ws/src/base_controller
2. $ mkdir src
3. $ vim src/base_controller.cpp

代码如下:
    /******************************************************************
    串口通信说明：
    1.写入串口
    （1）内容：左右轮速度，单位为mm/s
    （2）格式：10个字节,[右轮速度４字节][左轮速度４字节][结束符"\n"1个字节]
    2.读取串口
    （1）内容：小车线速度，角速度，单位依次为：mm/s,rad/s
    *******************************************************************/

1. #include "ros/ros.h"
2. #include <geometry_msgs/Twist.h>
3. #include <string>
4. #include <iostream>
5. #include <cstdio>
6. #include <unistd.h>
7. #include <math.h>
8. #include "serial/serial.h"
9. using std::ios;
10. using std::string;
11. using std::exception;
12. using std::cout;
13. using std::cerr;
14. using std::endl;
15. using std::vector;
16. float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
17. float D = 0.2680859f ;    //两轮间距，单位是m
18. float linear_temp=0,angular_temp=0;//暂存的线速度和角速度
19. unsigned char data_terminal=0x0a;  //“/n"字符
20. unsigned char speed_data[9]={0};   //要发给串口的数据
21. union floatData         //union的作用将较大的对象分解成组成这个对象的各个字节
22. {
23. float d;
24. unsigned char data[4];
25. }right_speed_data,left_speed_data;
26. /************************************************************/
27. void callback(const geometry_msgs::Twist& cmd_input)//订阅/cmd_vel主题回调函数
28. {
29. string port("/dev/ttyUSB0");    //小车串口号
30. unsigned long baud = 115200;    //小车串口波特率
31. serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
32. angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
33. linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
34. //将转换好的小车速度分量为左右轮速度
35. left_speed_data.d = linear_temp- 0.5f*angular_temp*D ;
36. right_speed_data.d = linear_temp+ 0.5f*angular_temp*D ;
37. //存入数据到要发布的左右轮速度消息
38. left_speed_data.d*=ratio;   //放大１０００倍，mm/s
39. right_speed_data.d*=ratio;//放大１０００倍，mm/s
40. cout<<"angular_temp = "<< angular_temp << endl;
41. cout<<"linear_temp = "<< linear_temp<<endl;
42. cout<<"left_speed_data.d = "<< left_speed_data.d<<endl;
43. cout<<"right_speed_data.d = "<< right_speed_data.d << endl;
44. for(int i=0;i<4;i++)    //将左右轮速度存入数组中发送给串口
45. {
46. speed_data[i]=right_speed_data.data[i];
47. speed_data[i+4]=left_speed_data.data[i];
48. }
49. //在写入串口的左右轮速度数据后加入”/n“
50. speed_data[8]=data_terminal;
51. //speed_data[9]=data_terminal1;
52. //写入数据到串口
53. for(int i=0;i<9;i++){
54. cout.setf(ios::hex,ios::basefield);//设置十六进制显示数值
55. cout.setf(ios::showbase|ios::uppercase);//设置0x头和大写
56. cout<<"s_i = "<<(int)speed_data[i]<<endl;
57. }
58. my_serial.write(speed_data,10);
59. }
60. int main(int argc, char **argv)
61. {
62. ros::init(argc, argv, "base_controller");
63. ros::NodeHandle n;
64. ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅cmd_vel主题
65. ros::spin();//周期执行
66. return 0;
67. }

修改CMakeList.txt:

1. cmake_minimum_required(VERSION 2.8.3)
2. project(ros_car_pkg)
3. find_package(catkin REQUIRED COMPONENTS
4. message_generation
5. roscpp
6. rospy
7. std_msgs
8. serial
9. tf
10. nav_msgs
11. )
12. include_directories(
13. # include
14. ${catkin_INCLUDE_DIRS}
15. ${serial_INCLUDE_DIRS}
16. )
17. add_message_files(FILES MsgCar.msg)
18. generate_messages(DEPENDENCIES std_msgs)
19. add_executable(base_controller src/base_controller.cpp)
20. target_link_libraries(base_controller ${catkin_LIBRARIES})
21. add_dependencies(base_controller ros_car_pkg_generate_messages_cpp)
22. catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs)

单独编译ros_car_pkg包:

1. $ catkin_make -DCATKIN_WHITELIST_PACKAGES='ros_car_pkg'

二、控制原理：

• 当我们按下键盘时，teleop_twist_keyboard 包会发布 /cmd_vel 发布速度主题
• 在 base_controller 节点订阅这个话题，接收速度数据，转换成字节数据，然后写入串口
• 通过USB转串口线，连接到板子的UART串口，在beaglebone中读取串口数据
• 将串口数据转换成pwm信号，并加载设备树，控制电机运转，从而实现键盘控制小车的移动

1.设置beaglebone串口：

查询手册，选择UART4，其中TXD(发送数据引脚)为P9-13，RXD(接受数据引脚)p9-11
    实验室的usb转串口线为：蓝色对应数据接受端，接RXD;白色对应发送数据端，接TXD，      黑色和红色一定要悬空！！！
    将串口线的USB端插上电脑，启动板子，在ubuntu中登录板子

$ ssh root@192.168.7.2
    测试的时候先手动加载UART4设备树(后面串口配置成功之后,可以通过程序直接启动）    
        $ cd /sys/devices/bone_capemgr.9/
        $ echo BB-UART4 > slots    
        $ cat slots
    如果加载成功，在/dev下会出现ttyO4串口
        $ cd /dev    
        $ ls tty*
    先使用screen或者minicom监测该串口
        $ sudo screen /dev/ttyO4 115200

设置ubuntu中的ros串口：
    如果usb转串口线没坏的话，在/dev下看到ttyUSB0
        $ cd /dev    
        $ ls tty*
        $ sudo chmod 666 /dev/ttyUSB0 
    如果该步骤设置成功的话，通过往ros串口发数据，可以在上述screen监测的串口看到发送的数据
        $ echo "Hello HFUT" > /dev/ttyUSB0

至此，串口设置完成

三、建立ros通信：

1.ubuntu中的ros端操作：

1. $ roscore
2. $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
3. $ rosrun ros_car_pkg base_controller

2.beaglebone终端操作:
    运行代码加载设备树并读串口数据用于控制PWM，进而控制小车运动

1. #include<stdio.h>
2. #include<fcntl.h>
3. #include<unistd.h>
4. #include<termios.h>
5. #define SLOTS   "/sys/devices/bone_capemgr.9/slots"
6. int main()
7. {
8. int fd, count_r,count_t,i;
9. unsigned char buff[100];  // the reading & writing buffer
10. struct termios opt;       //uart  confige structure
11. //加载设备树
12. if ((fd = open(SLOTS, O_WRONLY)) < 0)
13. {
14. perror("SLOTS: Failed to open the file. \n");
15. return -1;
16. }
17. if ((count_t = write(fd, "BB-UART4",8))<0) //8ge zi fu
18. {
19. perror("SLOTS:Failed to write to the file\nFailed to mount the UART4");
20. return -1;
21. }
22. close(fd);
23. //设置串口
24. if ((fd = open("/dev/ttyO4", O_RDWR | O_NOCTTY )) < 0)
25. {
26. perror("UART: Failed to open the UART device:ttyO4.\n");
27. return -1;
28. }
29. tcgetattr(fd, &opt); // get the configuration of the UART
30. opt.c_cflag = B115200| CS8 | CREAD | CLOCAL;
31. // 115200 baud, 8-bit, enable receiver, no modem control lines
32. opt.c_iflag = IGNPAR | ICRNL;
33. // ignore partity errors, CR -> newline
34. opt.c_iflag &= ~(IXON | IXOFF | IXANY);
35. //turn off software stream control
36. opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
37. tcflush(fd,TCIOFLUSH);        // 清理输入输出缓冲区
38. tcsetattr(fd, TCSANOW, &opt); // changes occur immmediately
39. //读串口
40. while(1)
41. {
42. if ((count_r = read(fd,(void*)buff,100))<0){
43. perror("ERR:No data is ready to be read\n");
44. return -1;
45. }
46. if (count_r == 0){
47. printf("ERR:No data is ready to be read\n");
48. }
49. else
50. {
51. printf("The following was read in [%d]:%X\n",count_r,buff); //具体格式是字节
52. }
53. //发送PWM
54. if (buff != NULL)
55. {
56. if ((fd = open(SLOTS, O_WRONLY)) < 0)
57. {
58. perror("SLOTS: Failed to open the file. \n");
59. return -1;
60. }
61. if ((count_t = write(fd, "bone_pwm_P9_22",14))<0) //8ge zi fu
62. {
63. perror("SLOTS:Failed to write to the file\nFailed to mount the bone_pwm_P9_22");
64. return -1;
65. }
66. if ((count_t = write(fd, "bone_pwm_P9_16",14))<0) //8ge zi fu
67. {
68. perror("SLOTS:Failed to write to the file\nFailed to mount the bone_pwm_P9_16");
69. return -1;
70. }
71. if ((count_t = write(fd, "am33xx_pwm",10))<0) //8ge zi fu
72. {
73. perror("SLOTS:Failed to write to the file\nFailed to mount the PWM");
74. return -1;
75. }
76. close(fd);
77. if ((fd = open(p922,O_WRONLY))<0)
78. {
79. perror("p922: Failed to open the file. \n");
80. return -1;
81. }
82. if((count_t= write(fd,"1",1))<0) //8ge zi fu
83. {
84. perror("p922run:Failed to write to the file\nFailed to mount the p9223");
85. return -1;
86. }
87. close(fd);
88. if ((fd = open(p916,O_WRONLY))<0)
89. {
90. perror("p916: Failed to open the file. \n");
91. return -1;
92. }
93. if((count_t= write(fd,"1",1))<0) //8ge zi fu
94. {
95. perror("p916run:Failed to write to the file\nFailed to mount the p9223");
96. return -1;
97. }
98. close(fd);
99. usleep(50000000);
100. }
101. return 0;
102. }