Turtlebot3 Jetson NX在ROS环境下键盘控制小车移动旋转 - 配置环境以及控制设置

ROS 工作环境配置

创建工作空间

mkdir -p ~/catkin_ws/src #src名不可变；-p：递归创建所有层级目录
cd ~/catkin_ws/src
catkin_init_workspace #把当前文件夹src初始化成ros工作空间（属性变化）

编译工作空间

cd ~/catkin_ws/
catkin_make #编译src下所有功能包的源码；生成build、devel

创建功能包

cd ~/catkin_ws/src #功能包要放在src中

配置功能包程序依赖

catkin_create_pkg base_controller roscpp

第三方库

下载键盘控制 ROS 包

cd ~/catkin_ws/src
git clone http://github.com/ncnynl/teleop_twist_keyboard.git
将 teleop_twist_keyboard.py 设置为可执行文件

下载串口通信的ROS包

cd ~/catkin_ws/src
git clone http://github.com/ncnynl/serial.git

控制程序

$ cd ~/catkin_ws/src
$ catkin_create_pkg base_controller roscpp
$ cd catkin_ws/src/base_controller
$ mkdir src 
$ touch src/base_controller.cpp
$ gedit src/base_controller.cpp

base_controller.cpp

/* base_controller.cpp */
#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"
#include<fcntl.h>
#include<unistd.h> 
#include<termios.h> 
#include<stdio.h>   
#include<string.h>

using namespace std;
/*****************************************************************************/
float D = 0.595f;    //两轮间距，单位是m
float linear_temp = 9, angular_temp = 9;//暂存的线速度和角速度
float reductionSpeedRatio = 13.75f; //减速比
/****************************************************/
string rec_buffer;  //串口数据接收变量

float left_speed = 0, right_speed = -0, left_speed_temp, right_speed_temp;
int left_dir = 0X00, right_dir = 0X00;
/****************************************************/
int fd;
int openUart(int comport);
int uartInit(int nSpeed, int nBits, char nEvent, int nStop);
void uartSend(int* send_buf, int length);
void uartRead(char receive_buf[], int length);
/************************************************************/
void callback(const geometry_msgs::Twist& cmd_input)//订阅/cmd_vel主题回调函数
{
	fd = openUart(1); //打开串口
	uartInit(115200, 8, 'n', 1); //波特率为115200，无奇偶校验,波特率需与下位机相匹配

	cout << "linear_temp: " << linear_temp << "    " << "angular_temp: " << angular_temp << endl;

	linear_temp = cmd_input.linear.x;//获取/cmd_vel的线速度.m/s
	angular_temp = cmd_input.angular.z;//获取/cmd_vel的角速度,rad/s
	
	cout << "linear_temp: " << linear_temp << "    " << "angular_temp: " << angular_temp << endl;
		
	//利用弧长计算公式，计算左右轮速度
	left_speed = (linear_temp - 0.5f * angular_temp * D) * 1000; //mm/s
	right_speed = (linear_temp + 0.5f * angular_temp * D) * 1000; //mm/s
	
	left_dir = left_speed >= 0 ? 0X00 : 0X01;
	right_dir = right_speed >= 0 ? 0X00 : 0X01;

	left_speed_temp = abs(left_speed) * reductionSpeedRatio + 0.5f; //精度？;mm/s
	right_speed_temp = abs(right_speed) * reductionSpeedRatio + 0.5f;

	cout << "right_speed: " << left_speed_temp << "    " << "left_speed: " << right_speed_temp << endl;
	cout << "right_dir: " << right_dir << "    " << "left_dir: " << left_dir << endl;

	int speed_data[10] = { 0xff, 0xfe,
						  static_cast<int>(left_speed_temp), static_cast<int>(right_speed_temp), //速度：右车轮， 左车轮
						  right_dir, left_dir, //方向：0X00 前进 0X01后退；十进制自动转为十六进制？
						  0x00, 0x00, 0x00, 0x00 }; //要发给串口的数据

	int* run = speed_data;
	for (run = speed_data; run < (speed_data + 10); run++) //开始发送数据
	{
		uartSend(run, 1);
	}
	cout << endl << endl;

	close(fd);
}

int main(int argc, char** argv)
{
	fd = openUart(1); //打开串口
	uartInit(115200, 8, 'n', 1); //波特率为115200，无奇偶校验,波特率需与下位机相匹配

	ros::init(argc, argv, "base_controller");//初始化串口节点
	ros::NodeHandle n;  //定义节点进程句柄

	ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题

	ros::spin();

	close(fd);

	return 0;
}

/******************TX2串口******************************************/
int openUart(int comport)
{
	const char* dev[] = { "/dev/ttyS0", "/dev/ttyTHS2" };
	//瑞泰科技只留出来两路串口，UART0为调试口，UART1为普通串口，所以咱们使用UART1
	if (comport == 0)
	{
		fd = open(dev[0], O_RDWR | O_NOCTTY | O_NDELAY);
		if (-1 == fd)
		{
			perror("Can't Open Serial Port");
			return (-1);
		}
	}
	else if (comport == 1)
	{
		fd = open(dev[1], O_RDWR | O_NOCTTY | O_NDELAY);
		if (-1 == fd)
		{
			perror("Can't Open Serial Port");
			return (-1);
		}
	}
	printf("fd-open=%d\n", fd);
	return fd;
}
int uartInit(int nSpeed, int nBits, char nEvent, int nStop)
{
	struct termios newtio, oldtio;
	/*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/
	if (tcgetattr(fd, &oldtio) != 0) {
		perror("SetupSerial 1");
		printf("tcgetattr( fd,&oldtio) -> %d\n", tcgetattr(fd, &oldtio));
		return -1;
	}
	bzero(&newtio, sizeof(newtio));
	/*步骤一，设置字符大小*/
	newtio.c_cflag |= CLOCAL | CREAD;
	newtio.c_cflag &= ~CSIZE;
	/*设置停止位*/
	switch (nBits)
	{
	case 7:
		newtio.c_cflag |= CS7;
		break;
	case 8:
		newtio.c_cflag |= CS8;
		break;
	}
	/*设置奇偶校验位*/
	switch (nEvent)
	{
	case 'o':
	case 'O': //奇数
		newtio.c_cflag |= PARENB;
		newtio.c_cflag |= PARODD;
		newtio.c_iflag |= (INPCK | ISTRIP);
		break;
	case 'e':
	case 'E': //偶数
		newtio.c_iflag |= (INPCK | ISTRIP);
		newtio.c_cflag |= PARENB;
		newtio.c_cflag &= ~PARODD;
		break;
	case 'n':
	case 'N':  //无奇偶校验位
		newtio.c_cflag &= ~PARENB;
		break;
	default:
		break;
	}
	/*设置波特率*/
	switch (nSpeed)
	{
	case 2400:
		cfsetispeed(&newtio, B2400);
		cfsetospeed(&newtio, B2400);
		break;
	case 4800:
		cfsetispeed(&newtio, B4800);
		cfsetospeed(&newtio, B4800);
		break;
	case 9600:
		cfsetispeed(&newtio, B9600);
		cfsetospeed(&newtio, B9600);
		break;
	case 115200:
		cfsetispeed(&newtio, B115200);
		cfsetospeed(&newtio, B115200);
		break;
	case 460800:
		cfsetispeed(&newtio, B460800);
		cfsetospeed(&newtio, B460800);
		break;
	default:
		cfsetispeed(&newtio, B9600);
		cfsetospeed(&newtio, B9600);
		break;
	}
	/*设置停止位*/
	if (nStop == 1)
		newtio.c_cflag &= ~CSTOPB;
	else if (nStop == 2)
		newtio.c_cflag |= CSTOPB;
	/*设置等待时间和最小接收字符*/
	newtio.c_cc[VTIME] = 0;
	newtio.c_cc[VMIN] = 0;
	/*处理未接收字符*/
	tcflush(fd, TCIFLUSH);
	/*激活新配置*/
	if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
	{
		perror("com set error");
		return -1;
	}
	printf("set done!\n");
	return 0;
}
void uartSend(int* send_buf, int length)
{
	int w;
	w = write(fd, send_buf, length);
	if (w == -1)
	{
		printf("Send failed!");
	}
	else
	{
		printf("Send success!");
	}
}
void uartRead(char receive_buf[], int length)
{
	int r;
	r = read(fd, receive_buf, strlen(receive_buf));
	for (int i = 0; i < r; i++)
	{
		printf("%c", receive_buf[i]);
	}

}

修改 CmakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  serial
  tf
  nav_msgs
)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${serial_INCLUDE_DIRS}
)
add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller 
  ${catkin_LIBRARIES}
)

编译并执行

$ cd ~/catkin_ws
$ catkin_make
$ roscore 
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
$ rosrun base_controller base_controller

如果没有问题就可以控制小车了

我这边还是有些问题的
参考以下连接：
参考1.
参考2.