使用TX2中ROS键盘控制STM32小车

于 2020-11-03 06:28:26 发布
阅读量1.4k 收藏 25
点赞数 1
文章标签： slam
香妃
本文链接：https://blog.csdn.net/wuxiangfei0011/article/details/109460962
版权

文章目录

TX2 与 STM32f103 硬件连接

在这里插入图片描述
在这里插入图片描述

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

查看串口号

$ ls -l /dev |grep ttyUSB （串口插上后才能查看到相应串口号）

第三方库

键盘控制 ROS 包

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

控制程序

$ 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 */
#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(
# include
       ${ catkin_INCLUDE_DIRS }
)

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

此时就可以使用键盘控制小车了

参考

https://blog.csdn.net/weixin_45438653/article/details/96884478?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-6.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-6.channel_param
https://blog.csdn.net/qq_39512995/article/details/84459035

香妃.
关注
  • 1
    点赞
  • 踩
  • 25
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值