一、所需工具包
1.ROS键盘包：teleop_twist_keyboard
2.ROS串口通讯包：serial
- $ cd ~/catkin_ws/src
- $ git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git
- $ git clone https://github.com/Forrest-Z/serial.git
- $ catkin_make
3.在ubuntu的ros中建立一个ros_car_pkg包:
- $ cd ~/catkin_ws/src
- $ catkin_create_pkg ros_car_pkg roscpp rospy std_msgs
4.新建 base_controller 文件:
- $ cd catkin_ws/src/base_controller
- $ mkdir src
- $ vim src/base_controller.cpp
代码如下:
/******************************************************************
串口通信说明：
1.写入串口
（1）内容：左右轮速度，单位为mm/s
（2）格式：10个字节,[右轮速度４字节][左轮速度４字节][结束符"\n"1个字节]
2.读取串口
（1）内容：小车线速度，角速度，单位依次为：mm/s,rad/s
*******************************************************************/
- #include "ros/ros.h"
- #include <geometry_msgs/Twist.h>
- #include <string>
- #include <iostream>
- #include <cstdio>
- #include <unistd.h>
- #include <math.h>
- #include "serial/serial.h"
- using std::ios;
- 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_terminal=0x0a; //“/n"字符
- unsigned char speed_data[9]={0}; //要发给串口的数据
- union floatData //union的作用将较大的对象分解成组成这个对象的各个字节
- {
- float d;
- unsigned char data[4];
- }right_speed_data,left_speed_data;
- /************************************************************/
- 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/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
- cout<<"angular_temp = "<< angular_temp << endl;
- cout<<"linear_temp = "<< linear_temp<<endl;
- cout<<"left_speed_data.d = "<< left_speed_data.d<<endl;
- cout<<"right_speed_data.d = "<< right_speed_data.d << endl;
- for(int i=0;i<4;i++) //将左右轮速度存入数组中发送给串口
- {
- speed_data[i]=right_speed_data.data[i];
- speed_data[i+4]=left_speed_data.data[i];
- }
- //在写入串口的左右轮速度数据后加入”/n“
- speed_data[8]=data_terminal;
- //speed_data[9]=data_terminal1;
- //写入数据到串口
- for(int i=0;i<9;i++){
- cout.setf(ios::hex,ios::basefield);//设置十六进制显示数值
- cout.setf(ios::showbase|ios::uppercase);//设置0x头和大写
- cout<<"s_i = "<<(int)speed_data[i]<<endl;
- }
- my_serial.write(speed_data,10);
- }
- int main(int argc, char **argv)
- {
- ros::init(argc, argv, "base_controller");
- ros::NodeHandle n;
- ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅cmd_vel主题
- ros::spin();//周期执行
- return 0;
- }
修改CMakeList.txt:
- cmake_minimum_required(VERSION 2.8.3)
- project(ros_car_pkg)
- find_package(catkin REQUIRED COMPONENTS
- message_generation
- roscpp
- rospy
- std_msgs
- serial
- tf
- nav_msgs
- )
- include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
- ${serial_INCLUDE_DIRS}
- )
- add_message_files(FILES MsgCar.msg)
- generate_messages(DEPENDENCIES std_msgs)
- add_executable(base_controller src/base_controller.cpp)
- target_link_libraries(base_controller ${catkin_LIBRARIES})
- add_dependencies(base_controller ros_car_pkg_generate_messages_cpp)
- catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs)
单独编译ros_car_pkg包:
- $ 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 [email protected]
测试的时候先手动加载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端操作：
- $ roscore
- $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
- $ rosrun ros_car_pkg base_controller
2.beaglebone终端操作:
运行代码加载设备树并读串口数据用于控制PWM，进而控制小车运动
- #include<stdio.h>
- #include<fcntl.h>
- #include<unistd.h>
- #include<termios.h>
- #define SLOTS "/sys/devices/bone_capemgr.9/slots"
- int main()
- {
- int fd, count_r,count_t,i;
- unsigned char buff[100]; // the reading & writing buffer
- struct termios opt; //uart confige structure
- //加载设备树
- if ((fd = open(SLOTS, O_WRONLY)) < 0)
- {
- perror("SLOTS: Failed to open the file. \n");
- return -1;
- }
- if ((count_t = write(fd, "BB-UART4",8))<0) //8ge zi fu
- {
- perror("SLOTS:Failed to write to the file\nFailed to mount the UART4");
- return -1;
- }
- close(fd);
- //设置串口
- if ((fd = open("/dev/ttyO4", O_RDWR | O_NOCTTY )) < 0)
- {
- perror("UART: Failed to open the UART device:ttyO4.\n");
- return -1;
- }
- tcgetattr(fd, &opt); // get the configuration of the UART
- opt.c_cflag = B115200| CS8 | CREAD | CLOCAL;
- // 115200 baud, 8-bit, enable receiver, no modem control lines
- opt.c_iflag = IGNPAR | ICRNL;
- // ignore partity errors, CR -> newline
- opt.c_iflag &= ~(IXON | IXOFF | IXANY);
- //turn off software stream control
- opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
- tcflush(fd,TCIOFLUSH); // 清理输入输出缓冲区
- tcsetattr(fd, TCSANOW, &opt); // changes occur immmediately
- //读串口
- while(1)
- {
- if ((count_r = read(fd,(void*)buff,100))<0){
- perror("ERR:No data is ready to be read\n");
- return -1;
- }
- if (count_r == 0){
- printf("ERR:No data is ready to be read\n");
- }
- else
- {
- printf("The following was read in [%d]:%X\n",count_r,buff); //具体格式是字节
- }
- //发送PWM
- if (buff != NULL)
- {
- if ((fd = open(SLOTS, O_WRONLY)) < 0)
- {
- perror("SLOTS: Failed to open the file. \n");
- return -1;
- }
- if ((count_t = write(fd, "bone_pwm_P9_22",14))<0) //8ge zi fu
- {
- perror("SLOTS:Failed to write to the file\nFailed to mount the bone_pwm_P9_22");
- return -1;
- }
- if ((count_t = write(fd, "bone_pwm_P9_16",14))<0) //8ge zi fu
- {
- perror("SLOTS:Failed to write to the file\nFailed to mount the bone_pwm_P9_16");
- return -1;
- }
- if ((count_t = write(fd, "am33xx_pwm",10))<0) //8ge zi fu
- {
- perror("SLOTS:Failed to write to the file\nFailed to mount the PWM");
- return -1;
- }
- close(fd);
- if ((fd = open(p922,O_WRONLY))<0)
- {
- perror("p922: Failed to open the file. \n");
- return -1;
- }
- if((count_t= write(fd,"1",1))<0) //8ge zi fu
- {
- perror("p922run:Failed to write to the file\nFailed to mount the p9223");
- return -1;
- }
- close(fd);
- if ((fd = open(p916,O_WRONLY))<0)
- {
- perror("p916: Failed to open the file. \n");
- return -1;
- }
- if((count_t= write(fd,"1",1))<0) //8ge zi fu
- {
- perror("p916run:Failed to write to the file\nFailed to mount the p9223");
- return -1;
- }
- close(fd);
- usleep(50000000);
- }
- return 0;
- }