ROS机器人控制 —— 键盘方向控制
文章目录
- 1. 键盘控制C++实现(x、y、z、theta)
- 2. 键盘控制Python实现(x、y、z、theta)
- 代码1
- 代码2
- 3.键盘笛卡尔方向控制(x、y、z)
- 4. 总结
通常情况下,对于机械臂的笛卡尔系下的控制或者小车的控制,我们在测试的时候可以通过键盘来实现,以下给出博主在网上找到的源码,能避免重复造轮子,更快的达到键盘控制的目的。
值得注意的是,以下代码左右键均是小车控制的代码,左右方向控制的是theta,因此对于机械臂的笛卡尔系控制还需要进一步修改代码。
1. 键盘控制C++实现(x、y、z、theta)
说明: 在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制1。
/** =====================================================================================* COPYRIGHT NOTICE* Copyright (c) 2013 HUST-Renesas Lab* ALL rights reserved.*//** * @file keyboard.cpp* @brief robot keyboard control* @version 0.1* @date 2013/5/23 15:22:40* @author Hu Chunxu , huchunxu@hust.edu.cn*//* ==================================================================================* @0.1 Hu Chunxu 2013/5/23 15:22:40 create orignal file* =====================================================================================*/#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>#include <boost/thread/thread.hpp>
#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/* 带有shift键 */
#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_;ros::NodeHandle n_;ros::Publisher pub_;public:SmartCarKeyboardTeleopNode(){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::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();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;/** * 从终端中获取按键 * 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实现(x、y、z、theta)
代码1
说明: 在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制1。
#!/usr/bin/env python
# -*- coding: utf-8 -*
# =====================================================================================
# COPYRIGHT NOTICE
# Copyright (c) 2013 HUST-Renesas Lab
# ALL rights reserved.
#
# @file teleop
# @brief robot keyboard control
# @version 0.1
# @date 2013/5/23 15:34:40
# @author Hu Chunxu , huchunxu@hust.edu.cn
# ==================================================================================
# @0.1 Hu Chunxu 2013/5/23 create orignal file
# =====================================================================================
import os
import sys
import tty, termiosimport roslib; roslib.load_manifest('smartcar_teleop')
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String# 全局变量
cmd = Twist()
pub = rospy.Publisher('cmd_vel', Twist)def keyboardLoop():#初始化rospy.init_node('smartcar_teleop')# Set rospy to exectute a shutdown function when exiting rate = rospy.Rate(rospy.get_param('~hz', 1))#速度变量walk_vel_ = rospy.get_param('walk_vel', 0.5)run_vel_ = rospy.get_param('run_vel', 1.0)yaw_rate_ = rospy.get_param('yaw_rate', 1.0)yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)max_tv = walk_vel_max_rv = yaw_rate_#显示提示信息print "Reading from keyboard"print "Use WASD keys to control the robot"print "Press Caps to move faster"print "Press q to quit"#读取按键循环while not rospy.is_shutdown():fd = sys.stdin.fileno()old_settings = termios.tcgetattr(fd)#不产生回显效果old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHOtry :tty.setraw( fd )ch = sys.stdin.read( 1 )finally :termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)if ch == 'w':max_tv = walk_vel_speed = 1turn = 0elif ch == 's':max_tv = walk_vel_speed = -1turn = 0elif ch == 'a':max_rv = yaw_rate_speed = 0turn = 1elif ch == 'd':max_rv = yaw_rate_speed = 0turn = -1elif ch == 'W':max_tv = run_vel_speed = 1turn = 0elif ch == 'S':max_tv = run_vel_speed = -1turn = 0elif ch == 'A':max_rv = yaw_rate_run_speed = 0turn = 1elif ch == 'D':max_rv = yaw_rate_run_speed = 0turn = -1elif ch == 'q':exit()else:max_tv = walk_vel_max_rv = yaw_rate_speed = 0turn = 0#发送消息cmd.linear.x = speed * max_tv;cmd.angular.z = turn * max_rv;pub.publish(cmd)rate.sleep()#停止机器人stop_robot();def stop_robot():cmd.linear.x = 0.0cmd.angular.z = 0.0pub.publish(cmd)if __name__ == '__main__':try:keyboardLoop()except rospy.ROSInterruptException:pass
代码2
说明2:
For Holonomic mode (strafing), hold down the shift key:
U ---------- I ---------- O
J ---------- K ---------- L
M ---------- < ---------- >
t : up (+z)
b : down (-z)
其他按键 : stop
q/z : 最大速度增加/减少10%
w/x : 仅线性速度增加/减少10%
e/c : 只增加/减少角速度10%
CTRL-C 退出
#!/usr/bin/env python
# -*- coding: utf-8 -*from __future__ import print_functionimport roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospyfrom geometry_msgs.msg import Twistimport sys, select, termios, ttymsg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:u i oj k lm , .For Holonomic mode (strafing), hold down the shift key:
---------------------------U I OJ K LM < >t : up (+z)
b : down (-z)anything else : stopq/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%CTRL-C to quit
"""moveBindings = {'i':(1,0,0,0),'o':(1,0,0,-1),'j':(0,0,0,1),'l':(0,0,0,-1),'u':(1,0,0,1),',':(-1,0,0,0),'.':(-1,0,0,1),'m':(-1,0,0,-1),'O':(1,-1,0,0),'I':(1,0,0,0),'J':(0,1,0,0),'L':(0,-1,0,0),'U':(1,1,0,0),'<':(-1,0,0,0),'>':(-1,-1,0,0),'M':(-1,1,0,0),'t':(0,0,1,0),'b':(0,0,-1,0),}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())select.select([sys.stdin], [], [], 0)key = sys.stdin.read(1)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keydef vels(speed,turn):return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)rospy.init_node('teleop_twist_keyboard')speed = rospy.get_param("~speed", 0.5)turn = rospy.get_param("~turn", 1.0)x = 0y = 0z = 0th = 0status = 0try:print(msg)print(vels(speed,turn))while(1):key = getKey()if key in moveBindings.keys():x = moveBindings[key][0]y = moveBindings[key][1]z = moveBindings[key][2]th = moveBindings[key][3]elif key in speedBindings.keys():speed = speed * speedBindings[key][0]turn = turn * speedBindings[key][1]print(vels(speed,turn))if (status == 14):print(msg)status = (status + 1) % 15else:x = 0y = 0z = 0th = 0if (key == '\x03'):breaktwist = Twist()twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turnpub.publish(twist)except Exception as e:print(e)finally: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)
3.键盘笛卡尔方向控制(x、y、z)
基于2中的代码2来实现
#!/usr/bin/env python
# -*- coding: utf-8 -*from __future__ import print_functionimport roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospyfrom geometry_msgs.msg import Twistimport sys, select, termios, ttymsg = """
---------------------------I J K L< I : +x J : +y T : +z
< : -x L : -y B : -zK : stop其他按键 : stopq/z : 最大速度增加/减少10%
w/x : 仅线性速度增加/减少10%
e/c : 只增加/减少角速度10%CTRL-C 退出
---------------------------
"""moveBindings = {'i':(1,0,0,0),'j':(0,1,0,0),'l':(0,-1,0,0),',':(-1,0,0,0),'I':(1,0,0,0),'J':(0,1,0,0),'L':(0,-1,0,0),'<':(-1,0,0,0),'t':(0,0,1,0),'b':(0,0,-1,0),'T':(0,0,1,0),'B':(0,0,-1,0),}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())select.select([sys.stdin], [], [], 0)key = sys.stdin.read(1)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keydef vels(speed,turn):return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)rospy.init_node('teleop_twist_keyboard')speed = rospy.get_param("~speed", 0.05)turn = rospy.get_param("~turn", 0.1)x = 0y = 0z = 0th = 0status = 0try:print(msg)print(vels(speed,turn))while(1):key = getKey()if key in moveBindings.keys():x = moveBindings[key][0]y = moveBindings[key][1]z = moveBindings[key][2]th = moveBindings[key][3]elif key in speedBindings.keys():speed = speed * speedBindings[key][0]turn = turn * speedBindings[key][1]print(vels(speed,turn))if (status == 14):print(msg)status = (status + 1) % 15else:x = 0y = 0z = 0th = 0if (key == '\x03'):breaktwist = Twist()twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turnpub.publish(twist)except Exception as e:print(e)finally: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)
4. 总结
在ROS下进行控制时,首先启动键盘控制节点,然后订阅相应的话题即可。
以3为例:
首先启动ROS Master:
roscore
运行该节点:
rosrun teleop_twist_keyboard teleop_twist_keyboard_descartes.py
查看话题列表:
rostopic list
查看话题内容:
rostopic echo cmd_vel
查看
/cmd_vel
话题的消息类型:rostopic type cmd_vel
订阅
/cmd_vel
话题后根据消息文件接收数据即可
http://www.guyuehome.com/253 ↩︎ ↩︎
https://github.com/ncnynl/teleop_twist_keyboard ↩︎
ROS机器人控制 —— 键盘方向控制相关推荐
- 【从零开始的ROS四轴机械臂控制】(六)- 逻辑控制节点
[从零开始的ROS四轴机械臂控制(六)] 九.逻辑控制节点 1.运动控制方法 (1)逆向运动学 (2)反馈控制 2.各节点之间的联系 3.相关程序 (1)img_process节点 (2)arm_co ...
- Cesium 键盘鼠标控制相机漫游(源码+原理讲解)
Cesium 键盘鼠标控制相机漫游(源码+原理讲解) 在各大博客平台上,Cesium使用键盘控制相机漫游的源码已经有不少人贴出源码,本人在浏览这些源码的过程中发现大家采用的方式基本一致,大部分代码都是 ...
- 基于paddledetection在ROS中搭建红绿灯检测控制车模运动(3)—— 使用训练结果在ros中仿真红绿灯控制小车运动
前言 第一节: 基于paddledetection在ROS中搭建红绿灯检测控制车模运动(1)-- 环境搭建准备以及软件安装 第二节: 基于paddledetection在ROS中搭建红绿灯检测控制车模 ...
- ROS 机器人控制APP
更多创客作品,请关注笔者网站园丁鸟,搜集全球极具创意,且有价值的创客作品 最近利用网上开源APPhttps://github.com/mtbii/RobotCA修改扩展了一个ROS机器人控制APP,如 ...
- 刀锋铁骑服务器维护,刀锋铁骑方向控制教程 教你如何灵活走动
刀锋铁骑方向控制教程 教你如何灵活走动 刀锋铁骑方向控制教程,对于新手来说进入游戏之后最想了解的应该就是方向到底要怎么控制了,下面我们就来看看吧,希望对你有所帮助.在铁骑中,近战兵种无论是攻击还是防御 ...
- 从零开始的ROS四轴机械臂控制-目录
[从零开始的ROS四轴机械臂控制](一)- 实际模型制作.Solidworks文件转urdf与rviz仿真 一.模型制作 1.实际模型制作 2.Solidworks模型制作 二.Solidworks文 ...
- 【从零开始的ROS四轴机械臂控制】(七)- ROS与arduino连接
从零开始的ROS四轴机械臂控制(七) 十.ROS与arduino连接 1.虚拟机与arduino的连接 (1)arduino连接与IDE (2)PCA9685模块支持与测试 2.ROS与arduino ...
- 【从零开始的ROS四轴机械臂控制】(五)- 构建运动控制服务
[从零开始的ROS四轴机械臂控制(五)] 八.运动控制节点 1.定义服务GoToPosition.srv 2.修改CMakeLists.txt 3.修改package.xml 4.构建包 5.arm_ ...
- 【从零开始的ROS四轴机械臂控制】(四)- ros、gazebo与opencv,图像处理节点
[从零开始的ROS四轴机械臂控制(四)] 七.图像处理节点 1.节点功能与实现方法 2.iamge_process 相关程序 部分程序解释 3.节点运行与测试 七.图像处理节点 1.节点功能与实现方法 ...
最新文章
- C#程序设计语言Version2.0简介
- django 快速实现文件上传
- 【AI应用】三分钟,带你了解英特尔明年重点发力的这八大方向
- 常用python分析数据包pipinstallnumpy_安装numpy和matplotlib时,pip依赖关系解析失败
- 【AcWing 243. 一个简单的整数问题2】
- Linux驱动开发常用头文件
- fork()使用(一)
- 怎么将oracle的sql文件转换成mysql的sql文件
- 斗地主AI算法——第十章の被动出牌(4)
- [android] 帧动画和补间动画
- 【Python实例第14讲】普通判别分析与缩水判别分析
- html pt兼容,FCKEditor,一个HTML可视化编辑器,兼容很好
- C# 图片验证码简单例子
- 程序员应该每年更新自己的简历
- 历时三月,致远互联从“鸟巢”飞到哪了?
- 用scratch制作蓄力跳
- 手机浏览器呼出QQ聊天窗口,微信内置浏览器呼出qq
- 解决pycharm中画图工具报错无法显示问题
- 野火 步进电机 视频笔记
- 安装gosublime插件出现See the `Quirks` section of USAGE.md for info
热门文章
- C. 点和圆 (类与对象)
- 【ROM制作工具】小白如何进行ROM解包,精简,修改,授权,打包详细图文教程...
- 跨境电商属于外贸吗,Starday跨境电商靠谱吗?
- android 获取AndroidManifest.xml中的meta-data元素
- 微信、微博、贴吧平台涉嫌违反《网络安全法》被立案调查 审查
- 速卖通电脑办公行业什么产品好卖?解读2022速卖通重点招商品类及营销策略
- 我也来一个“羊了个羊”
- 微信消息记录如何恢复
- 安卓手机分辨率修改工具的使用方法
- delphi 获取打印机默认纸张_Delphi中如何控制打印方向,纸张大小等 | 学步园