ros----键盘控制机器人（2）【键盘控制文件书写】

一、前言

在上一篇我们着重写了键盘控制的整体思路和launch文件的书写，这一篇，我就着重写一下我从网上找到的键盘控制cpp代码和python代码。

let's go!!!!!!

二、代码集合

1、cpp代码

#include <termios.h>#include <signal.h>#include <math.h>#include <stdio.h>#include <stdlib.h>#include <sys/poll.h>#include <boost/thread/thread.hpp>
// ros/ros.h是一个实用的头文件，它引用了ROS系统中大部分常用的头文件
#include <ros/ros.h>#include <geometry_msgs/Twist.h>#define KEYCODE_W 0x77#define KEYCODE_A 0x61#define KEYCODE_S 0x73#define KEYCODE_D 0x64#define KEYCODE_A_CAP 0x41#define KEYCODE_D_CAP 0x44#define KEYCODE_S_CAP 0x53#define KEYCODE_W_CAP 0x57class SmartCarKeyboardTeleopNode {private:double walk_vel_;double run_vel_;double yaw_rate_;double yaw_rate_run_;geometry_msgs::Twist cmdvel_;//为这个进程的节点创建一个句柄,第一个创建的NodeHandle会为节点进行初始化,//最后一个销毁的 NodeHandle 则会释放该节点所占用的所有资源ros::NodeHandle n_;//初始化发布节点ros::Publisher pub_;public:SmartCarKeyboardTeleopNode() {//告诉master我们将要在cmd_vel(话题名)上发布geometry_msgs/Twist消息类型的消息//这样master就会告诉所有订阅了cmd_vel话题的节点,将要有数据发布．//第二个参数是发布序列的大小,如果我们发布消息的频率太高，//缓冲区中的消息在大于1000个的时候就会开始丢弃先前发布的消息//NodeHandle::advertise() 返回一个 ros::Publisher 对象,它有两个作用：//1) 它有一个 publish() 成员函数可以让你在topic上发布消息；//2) 如果消息类型不对,它会拒绝发布。pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);ros::NodeHandle n_private("~");n_private.param("walk_vel", walk_vel_, 0.5);n_private.param("run_vel", run_vel_, 1.0);n_private.param("yaw_rate", yaw_rate_, 1.0);n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);}~SmartCarKeyboardTeleopNode() {}void keyboardLoop();void stopRobot() {cmdvel_.linear.x = 0.0;cmdvel_.angular.z = 0.0;pub_.publish(cmdvel_);}};SmartCarKeyboardTeleopNode *tbk;
/*** 文件描述符* 内核（kernel）利用文件描述符（file descriptor）来访问文件。文件描述符是非负整数。* 标准输入（standard input）的文件描述符是 0，标准输出（standard output）是 1，标准错误（standard error）是 2。*/
int kfd = 0;
/** *  === struct termios ===*  tcflag_t c_iflag;  输入模式*　tcflag_t c_oflag;  输出模式 *　tcflag_t c_cflag;  控制模式 *  tcflag_t c_lflag;  本地模式*  cc_t c_cc[NCCS];   控制字符 */
struct termios cooked, raw;bool done;int main(int argc, char **argv) {//初始化ROS.它允许允许ROS通过命令进行名称重映射．//我们可以指定节点的名称，但是这里的名称必须是basename,名称内不能包含/等符号ros::init(argc, argv, "tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);SmartCarKeyboardTeleopNode tbk;//重新定义一个线程boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));//ros::spin()在调用后不会再返回，也就是你的主程序到这儿就不往下执行了，//而ros::spinOnce()后者在调用后还可以继续执行之后的程序。ros::spin();//中断线程t.interrupt();//由于线程中断，所以立即返回t.join();//速度置零tbk.stopRobot();tcsetattr(kfd, TCSANOW, &cooked);return (0);}void SmartCarKeyboardTeleopNode::keyboardLoop() {char c;double max_tv = walk_vel_;double max_rv = yaw_rate_;bool dirty = false;int speed = 0;int turn = 0;// get the console in raw mode  /** * 从终端中获取按键 * int tcgetattr(int fd, struct termios *termios_p);*/tcgetattr(kfd, &cooked);memcpy(&raw, &cooked, sizeof(struct termios));/*** c_lflag : 本地模式标志，控制终端编辑功能* ICANON: 使用标准输入模式* ECHO: 显示输入字符*/raw.c_lflag &= ~(ICANON | ECHO);/** * c_cc[NCCS]：控制字符，用于保存终端驱动程序中的特殊字符，如输入结束符等* VEOL: 附加的End-of-file字符* VEOF: End-of-file字符* */raw.c_cc[VEOL] = 1;raw.c_cc[VEOF] = 2;tcsetattr(kfd, TCSANOW, &raw);puts("Reading from keyboard");puts("Use WASD keys to control the robot");puts("Press Shift to move faster");/* ** struct pollfd {int fd;        文件描述符 short events;  等待的事件 short revents; 实际发生了的事件 };*/struct pollfd ufd;ufd.fd = kfd;ufd.events = POLLIN;for (;;) {boost::this_thread::interruption_point();// get the next event from the keyboard  int num;/*** poll:把当前的文件指针挂到设备内部定义的等待队列中。* unsigned int (*poll)(struct file * fp, struct poll_table_struct * table)*/if ((num = poll(&ufd, 1, 250)) < 0) {/*** perror( ) 用来将上一个函数发生错误的原因输出到标准设备(stderr)。* 参数s所指的字符串会先打印出,后面再加上错误原因字符串。* 此错误原因依照全局变量errno 的值来决定要输出的字符串。* */perror("poll():");return;} else if (num > 0) {if (read(kfd, &c, 1) < 0) {perror("read():");return;}} else {/* 每按下一次动一下 */if (dirty == true) {stopRobot();dirty = false;}continue;}switch (c) {case KEYCODE_W:max_tv = walk_vel_;speed = 1;turn = 0;dirty = true;break;case KEYCODE_S:max_tv = walk_vel_;speed = -1;turn = 0;dirty = true;break;case KEYCODE_A:max_rv = yaw_rate_;speed = 0;turn = 1;dirty = true;break;case KEYCODE_D:max_rv = yaw_rate_;speed = 0;turn = -1;dirty = true;break;case KEYCODE_W_CAP:max_tv = run_vel_;speed = 1;turn = 0;dirty = true;break;case KEYCODE_S_CAP:max_tv = run_vel_;speed = -1;turn = 0;dirty = true;break;case KEYCODE_A_CAP:max_rv = yaw_rate_run_;speed = 0;turn = 1;dirty = true;break;case KEYCODE_D_CAP:max_rv = yaw_rate_run_;speed = 0;turn = -1;dirty = true;break;default:max_tv = walk_vel_;max_rv = yaw_rate_;speed = 0;turn = 0;dirty = false;}cmdvel_.linear.x = speed * max_tv;cmdvel_.angular.z = turn * max_rv;pub_.publish(cmdvel_);}}

2、python代码

#!/usr/bin/env python
import rospyfrom geometry_msgs.msg import Twistimport sys, select, termios, tty#操作教程
msg = """
Control The Robot!
---------------------------
Moving around:u    i    oj    k    lm    ,    .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
#用于改变机器人运动方向的字典
moveBindings = {'i':(1,0),'o':(1,-1),'j':(0,1),'l':(0,-1),'u':(1,1),',':(-1,0),'.':(-1,1),'m':(-1,-1),}
#用于改变机器人运动速度的字典
speedBindings={'q':(1.1,1.1),'z':(.9,.9),'w':(1.1,1),'x':(.9,1),'e':(1,1.1),'c':(1,.9),}def getKey():tty.setraw(sys.stdin.fileno())#tty 模块定义了将 tty 放入 cbreak 和 raw 模式的函数。因为它需要 termios 模块，所以只能在 Unix 上运行。#tty 模块定义了以下函数：#tty.setraw(fd, when=termios.TCSAFLUSH)#将文件描述符 fd 的模式更改为 raw 。如果 when 被省略，则默认为 termios.TCSAFLUSH ，并传递给 termios.tcsetattr() 。#tty.setcbreak(fd, when=termios.TCSAFLUSH)#将文件描述符 fd 的模式更改为 cbreak 。如果 when 被省略，则默认为 termios.TCSAFLUSH ，并传递给 termios.tcsetattr() 。rlist, _, _ = select.select([sys.stdin], [], [], 0.1)#rlist则是键盘输入的信息列表#select()方法接收并监控3个通信列表#第一个是所有的输入的data,就是指外部发过来的数据#第2个是监控和接收所有要发出去的data(outgoing data)#第3个监控错误信息#判断是否接受到数据if rlist:key = sys.stdin.read(1)#读取终端上的交互输入else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)#termios.tcsetattr(fd, when, attributes)#fd获取终端上交互输入的返回列表#when  tcsanow立即更改，tcsadrain在传输所有排队的输出后更改，或tcsaflush在传输所有排队的输出并丢弃所有排队的输入后更改。return keyspeed = .2  #设置初始直行速度
turn = 1    #设置初始角速度def vels(speed,turn): #打印输入目前的设置的速度和角速度return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin) #返回一个包含文件描述符fd的tty属性的列表，用于getkey的第三个参数rospy.init_node('robot_teleop')#初始化节点pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=5)#发布话题x = 0  #代表机器人的前进和后退方向 1前进-1后退th = 0  #代表机器人的原地左转和右转方向  1左转-1右转status = 0 #调整加速减速的次数count = 0target_speed = 0   #目标直行速度 target_turn = 0     #目标角速度control_speed = 0   #实际运行直行速度control_turn = 0    #实际运行角速度try:print msg   #打印操作教程print vels(speed,turn)  #打印目前设置的最大速度和角速度while(1):key = getKey()  #获取终端上键盘的输入信息if key in moveBindings.keys(): #判断是否是控制移动方向的字典值x = moveBindings[key][0]th = moveBindings[key][1]count = 0elif key in speedBindings.keys():   #判断是否控制速度大小的字典值speed = speed * speedBindings[key][0]turn = turn * speedBindings[key][1]count = 0print vels(speed,turn)  #打印更改后的最大速度和角速度#超过14后重新打印操作教程if (status == 14):print msgstatus = (status + 1) % 15elif key == ' ' or key == 'k' :#其余命令或者是k，均是代表停止x = 0th = 0control_speed = 0 #实际直行速度给0control_turn = 0    #实际角速度给0else:count = count + 1  #未接受键盘信息时，保持停止if count > 4:x = 0th = 0if (key == '\x03'):#猜测是ctrl+c退出键盘控制breaktarget_speed = speed * x #目标直行速度target_turn = turn * th #目标角速度#控制机器人的加速和减速过程 0.02,0.1分别代表当前频率下的速度和角速度变化率，相当于加速度大小if target_speed > control_speed:control_speed = min( target_speed, control_speed + 0.02 )elif target_speed < control_speed:control_speed = max( target_speed, control_speed - 0.02 )else:control_speed = target_speedif target_turn > control_turn:control_turn = min( target_turn, control_turn + 0.1 )elif target_turn < control_turn:control_turn = max( target_turn, control_turn - 0.1 )else:control_turn = target_turntwist = Twist()#初始化twisttwist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turnpub.publish(twist)#发布实时状态的控制速度和角速度#print("loop: {0}".format(count))#print("target: vx: {0}, wz: {1}".format(target_speed, target_turn))#print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))except:print efinally:  #控制机器人处于停止状态twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.publish(twist)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

ros----键盘控制机器人（2）【键盘控制文件书写】相关推荐

机器人感知与控制关键技术及其智能制造应用
源自:自动化学报作者:王耀南江一鸣姜娇张辉谭浩然彭伟星吴昊天曾凯摘要智能机器人在服务国家重大需求, 引领国民经济发展和保障国防安全中起到重要作用, 被誉为"制造业皇冠 ...
ROS 教程2 机器人雷达建图蒙特卡洛定位导航路径规划仿真
ros 机器人雷达建图蒙特卡洛定位导航路径规划仿真 move_base gmapping acml 博文github 一.安装 turtlebot 移动机器人底座进行导航 1.安装系统依赖 ...
ROS键盘控制机器人
目录前言&&过程分析第一步第二步第三步第四步 PC端(上位机)代码远程机器人(下位机)代码前言&&过程分析当我们需要使用键盘控制机器人的时候,通常,分为 ...
ROSNOTE ：键盘控制机器人运动
ROS与底盘的通信协议: ROS平台与小车底盘通信一般是通过串口写入串口的内容是左右轮的速度从串口中读取到的是小车x,y坐标,方向角,角速度 ROS平台串口处理程序: 主要是写在base_con ...
八、键盘控制无人机 · 上（launch文件解读）
没想到又是这位博主,佩服佩服! 这里面的launch文件注释写的真的是非常详细! 转载自:https://blog.csdn.net/weixin_44917390/article/details/1 ...
ROS 教程3 机器人语音语音识别理解合成控制 ASR NLU TTS
机器人语音语音识别理解合成控制 ASR NLU TTS github 一.语音处理总体框架 1. 语音识别(ASR , Automatic Speech Recognition )2. 语义理解(N ...
机器人操作系统ROS（8）arbotix控制器控制小车运动
本文内容:在 Ubuntu 18.04 虚拟机中,基于安装配置好 ROS Ketinc上,进行 URDF 机器人建模的详细步,同时配置摄像. 说明:本文是之前的延伸,该节需要参考上节机器人操作系统RO ...
ROS小车应用：控制机器人做圆周运动
一.首先创建一个功能包跳转到src目录下 catkin_create_pkg 功能包名字进入目录,创建一个script目录(主要使用py来写) 然后创建一个py文件,取名draw_circle.p ...
qt ui界面无法移动控件_使用qt 键盘上的方向键只能控制ui界面上的按钮选择，不能实现我设定的功能...
已结贴√ 问题点数:20 回复次数:2 使用qt 键盘上的方向键只能控制ui界面上的按钮选择,不能实现我设定的功能我做的是一个贪吃蛇游戏,现在我想实现的功能是:按下键盘上的上下左右箭头的方向按键时, ...

ros----键盘控制机器人（2）【键盘控制文件书写】

ros----键盘控制机器人（2）【键盘控制文件书写】相关推荐

最新文章

热门文章