如果尝试过前面的例子,有没有感觉每次让机器人移动还要在终端里输入指令,这也太麻烦了,有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人键盘控制的代码,还是有所收获的,最后移植到了smartcar上,实验成功。
roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
rosmake
#!/usr/bin/env python
import roslib; roslib.load_manifest('smartcar_teleop')
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
class Teleop:
def __init__(self):
pub = rospy.Publisher('cmd_vel', Twist)
rospy.init_node('smartcar_teleop')
rate = rospy.Rate(rospy.get_param('~hz', 1))
self.cmd = None
cmd = Twist()
cmd.linear.x = 0.2
cmd.linear.y = 0
cmd.linear.z = 0
cmd.angular.z = 0
cmd.angular.z = 0
cmd.angular.z = 0.5
self.cmd = cmd
while not rospy.is_shutdown():
str = "hello world %s" % rospy.get_time()
rospy.loginfo(str)
pub.publish(self.cmd)
rate.sleep()
if __name__ == '__main__':Teleop()
rosrun smartcar_teleop teleop.py
rosbuild_add_boost_directories()
rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)
target_link_libraries(smartcar_keyboard_teleop boost_thread)
#include
#include
#include
#include
#include
#include
#include
#include
#include
#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 0x57
class 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("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;
int kfd = 0;
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;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
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 ufd;
ufd.fd = kfd;
ufd.events = POLLIN;
for(;;)
{
boost::this_thread::interruption_point();
// get the next event from the keyboard
int num;
if ((num = poll(&ufd, 1, 250)) < 0)
{
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_);
}
}
#!/usr/bin/env python
# -*- coding: utf-8 -*
import os
import sys
import tty, termios
import 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')
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.ECHO
try :
tty.setraw( fd )
ch = sys.stdin.read( 1 )
finally :
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
if ch == 'w':
max_tv = walk_vel_
speed = 1
turn = 0
elif ch == 's':
max_tv = walk_vel_
speed = -1
turn = 0
elif ch == 'a':
max_rv = yaw_rate_
speed = 0
turn = 1
elif ch == 'd':
max_rv = yaw_rate_
speed = 0
turn = -1
elif ch == 'W':
max_tv = run_vel_
speed = 1
turn = 0
elif ch == 'S':
max_tv = run_vel_
speed = -1
turn = 0
elif ch == 'A':
max_rv = yaw_rate_run_
speed = 0
turn = 1
elif ch == 'D':
max_rv = yaw_rate_run_
speed = 0
turn = -1
elif ch == 'q':
exit()
else:
max_tv = walk_vel_
max_rv = yaw_rate_
speed = 0
turn = 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.0
cmd.angular.z = 0.0
pub.publish(cmd)
if __name__ == '__main__':
try:
keyboardLoop()
except rospy.ROSInterruptException:
pass
----------------------------------------------------------------
欢迎大家转载我的文章。
转载请注明:转自古-月
http://blog.csdn.net/hcx25909
欢迎继续关注我的博客