ROS exploration Summary (8) -- Keyboard Control

If I tried the previous example, is it too troublesome to input commands to the terminal every time the robot moves? Is there a way to control the robot's movement through the keyboard? Answer room of course. I have studied several other robot keyboard-controlled code, but I still have some gains. I finally transplanted it to smartcar, and the experiment was successful.

1. Create a control package first. Create a separate package for the keyboard control:

roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpprosmake

If you have forgotten how to build the package, see: http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage
2. simple messages are published in robot simulation. What mainly controls Robot Movement is in robot simulation. What mainly controls Robot Movement is the Twist () structure, if we program to publish this structure into a topic through a program, we can naturally control the robot. Let's try it with a simple python. In the previous simulation, we used message publishing under the command line. Now we need to convert these commands into python code and encapsulate them into a single node. For the previous command line, we can easily write the following control code in the smartcar_teleop/scripts Folder:

#!/usr/bin/env pythonimport roslib; roslib.load_manifest('smartcar_teleop')import rospyfrom geometry_msgs.msg import Twistfrom std_msgs.msg import Stringclass 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()

Python code does not need to be compiled in ROS. Run the smartcar robot used in the previous tutorial, display it in rviz, create a terminal, and enter the following command:

rosrun smartcar_teleop teleop.py

You can also create a launch file to run:

<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>

Check rviz to see if the robot is moving!
3. Add the keyboard control. Of course, the frontend program is not what we need. We need the keyboard control. 1. Because ROS code is highly portable, the code controlled by the keyboard can be transplanted directly from other robot packages. Here I mainly refer to erratic_robot, the robot Code contains an erratic_teleop package, which can be directly transplanted for use.
Upload File, add the following code to the bottom of the file:

rosbuild_add_boost_directories()rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)target_link_libraries(smartcar_keyboard_teleop boost_thread)

After compilation, run the smartcar model. Re-open a terminal and open the keyboard control node:
In the terminal, press the W, S, D, A, and Shift keys in the keyboard to control the robot. The effect is as follows:

2. reuse because the code is directly copied. There are many variables related to the previous erratic robot. We make slight changes to the Code to make it readable for our robots.

#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 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;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_);    }}

Reference: http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation
Http://www.ros.org/wiki/simulator_gazebo/tutorials/teleoperraticsimulation3133 innovation although many robot keyboard control uses C ++ code, but considering the powerful python, we still need to try to write programs using python.
First, you need to understand the process of the above C ++ program. In the above program, we created a separate thread to read the input in the interrupt, and then published different speed and angle messages based on the input. The concept of thread is still relatively weak. In python, loop is used to replace the thread. Then, you only need to consider how to use python to process the input characters in the interrupt. By searching for information on the Internet, it is found that the APIs used are basically the same as those of C ++. The final program is as follows:

#! /Usr/bin/env python #-*-coding: UTF-8-* import osimport sysimport tty, termiosimport roslib; roslib. load_manifest ('smartcar _ teleop ') import rospyfrom geometry_msgs.msg import Twistfrom std_msgs.msg import String # global variable cmd = Twist () pub = rospy. publisher ('COMMAND _ vel ', Twist) def keyboardLoop (): # initialize rospy. init_node ('smartcar _ teleop ') rate = rospy. rate (rospy. get_param ('~ Hz ', 1) # speed variable pai_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 = pai_vel _ max_rv = yaw_rate _ # display prompt print "Reading from keyboard" print "Use WASD keys to control the robot" print "Press Caps to move faster" print "Press q to quit "# Read the key loop while not rospy. Is_shutdown (): fd = sys. stdin. fileno () old_settings = termios. tcgetattr (fd) # No echo effect is generated. 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 = pai_vel _ speed = 1 turn = 0 elif ch ='s ': max_ TV = pai_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 = pai_vel _ max_rv = yaw_rate _ speed = 0 turn = 0 # send the message cmd. linear. x = speed * max_ TV; cmd. angular. z = turn * max_rv; pub. publish (cmd) rate. sleep () # Stop the robot stop_robot (); def stop_robot (): cmd. linear. x = 0.0 cmd. angular. z = 0.0 pub. publish (cmd) if _ name _ = '_ main _': try: keyboardLoop () handle T rospy. ROSInterruptException: pass

Reference: http://blog.csdn.net/marising/article/details/3173848
Http://nullege.com/codes/search/termios.ICANON
Iv. node Relationship Diagram

Code package I have uploaded: http://download.csdn.net/detail/hcx25909/5496381 hope everyone to learn together, make common progress!

----------------------------------------------------------------

You are welcome to repost my article.

Reprinted, please note: transferred from ancient-month

Http://blog.csdn.net/hcx25909

Continue to follow my blog