搭建环境：XMWare Ubuntu14.04 ROS（indigo）
转载自古月居 转载连接：http://www.guyuehome.com/253
一、创建控制包
catkin_create-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
catkin_make
建包，参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage
二、简单的消息发布
在smartcar_teleop下 创建scripts文件夹，在其文件夹下创建teleop.py文件
#!/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:
"""docstring fos 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.x = 0
cmd.angular.y = 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()
先运行之前教程中用到的smartcar机器人，在rviz中进行显示
然后新建终端，输入如下命令：
sudo chmod +x teleop.py
rosrun smartcar_teleop teleop.py
可以建立一个launch文件（teleop.launch）运行
<launch>
<arg name="cmd_topic" default="cmd_vel" />
<node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">
<remap from="cmd_vel" to="$(arg cmd_topic)" />
</node>
</launch>
三、加入键盘控制
1、移植
首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：
add_executable(smartcar_keyboard_teleop src/keyboard.cpp)
target_link_libraries(smartcar_keyboard_teleop boost_thread ${catkin_LIBRARIES})
keyboard.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>
#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 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<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;
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)) <)
{
perror("poll():");
return;
}
else if(num > 0)
{
if(read(kfd, &c, 1) <)
{
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_);
}
}
编译完成后，运行smartcar模型。重新打开一个终端，打开键盘控制节点：
rosrun smartcar_teleop smartcar_keyboard_teleop
在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。