ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制

ROS探索总结(七)——smartcar源码上传

看到前面写的博客还是帮助了很多ROS的学习者,我感到非常荣幸。其实我也是一名ROS的新手,ROS的相关资料少,上手难度大,我现在也在摸索着学习,还希望大家都将自己的学习成果在网上或者ROS群里分享。

        我看到有些人在运行我前面写的smartcar程序,为了方便大家的学习,我这两天整理了一下代码,已经上传到csdn上,下载请见:

        http://download.csdn.net/detail/hcx25909/5487985

        代码的内容主要是前边用到的机器人smartcar的urdf文件,已经运行使用的launch文件,大家下载后可以直接编译使用,由于代码在我的计算机上编译时带有路径信息,所以大家编译的时候请使用下面的命令先清除再编译:

[cpp]  view plain  copy
  1. rosmake smartcar_description --pre-clean  


       代码中的urdf文件里室机器人的urdf描述,和上一篇博客中讲的是一样的,launch文件夹中主要有以下文件:

ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第1张图片

        第一个实在gazebo仿真中用到的,需要先运行gazebo,然后再运行。但是貌似有问题,我每次加载到gazebo中的时候似乎物理参数不对,机器人在gazebo里飞来飞去的,至今还没有解决。如果哪位仁兄发现问题了,还请多多指教。

        第二个文件是我在绘制模型的时候用到的,主要功能是在rviz中显示机器人模型,文件中的以下代码:

[cpp]  view plain  copy
  1. "gui" default="false" />  


        如果把false改为true,则会打开一个gui的调试界面,可以通过滑动条旋转车轮,如下图所示。默认是false。

ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第2张图片

        第三个文件是在方针的时候用到的,就是前面博客中使用的,会打开arbotix仿真器。

        根目录中还有rviz的配置文件urdf.vcg,只要大家在rviz中修改界面或者参数,推出的时候保存,就会自动修改这个文件了。

        config文件夹下的是仿真用到的配置文件。

 

PS:       

        代码中还有很多问题,请大家多多指教,共同进步。


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




ROS探索总结(八)——键盘控制

如果尝试过前面的例子,有没有感觉每次让机器人移动还要在终端里输入指令,这也太麻烦了,有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人键盘控制的代码,还是有所收获的,最后移植到了smartcar上,实验成功。

一、创建控制包

        首先,我们为键盘控制单独建立一个包:
[plain]  view plain  copy
  1. roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
  2. rosmake  


        如果你已经忘记了怎么建立包,请参考:http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

二、简单的消息发布

        在机器人仿真中,主要控制机器人移动的就是        在机器人仿真中,主要控制机器人移动的就是Twist()结构,如果我们编程将这个结构通过程序发布成topic,自然就可以控制机器人了。我们先用简单的python来尝试一下。
        之前的模拟中,我们使用的都是在命令行下进行的消息发布,现在我们需要把这些命令转换成python代码,封装到一个单独的节点中去。针对之前的命令行,我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码:
[python]  view plain  copy
  1. #!/usr/bin/env python  
  2. import roslib; roslib.load_manifest('smartcar_teleop')  
  3. import rospy  
  4. from geometry_msgs.msg import Twist  
  5. from std_msgs.msg import String  
  6.   
  7. class Teleop:  
  8.     def __init__(self):  
  9.         pub = rospy.Publisher('cmd_vel', Twist)  
  10.         rospy.init_node('smartcar_teleop')  
  11.         rate = rospy.Rate(rospy.get_param('~hz'1))  
  12.         self.cmd = None  
  13.       
  14.         cmd = Twist()  
  15.         cmd.linear.x = 0.2  
  16.         cmd.linear.y = 0  
  17.         cmd.linear.z = 0  
  18.         cmd.angular.z = 0  
  19.         cmd.angular.z = 0  
  20.         cmd.angular.z = 0.5  
  21.   
  22.         self.cmd = cmd  
  23.         while not rospy.is_shutdown():  
  24.             str = "hello world %s" % rospy.get_time()  
  25.             rospy.loginfo(str)  
  26.             pub.publish(self.cmd)  
  27.             rate.sleep()  
  28.   
  29. if __name__ == '__main__':Teleop()  


       python代码在ROS重视不需要编译的。先运行之前教程中用到的smartcar机器人,在rviz中进行显示,然后新建终端,输入如下命令:
[plain]  view plain  copy
  1. rosrun smartcar_teleop teleop.py  


        也可以建立一个launch文件运行:
[plain]  view plain  copy
  1.   
  2.     
  3.     
  4.       
  5.     
  6.   


        在rviz中看一下机器人是不是动起来了!

三、加入键盘控制

        当然前边的程序不是我们要的,我们需要的键盘控制。

1、移植

        因为ROS的代码具有很强的可移植性,所以用键盘控制的代码其实可以直接从其他机器人包中移植过来,在这里我主要参考的是erratic_robot,在这个机器人的代码中有一个erratic_teleop包,可以直接移植过来使用。
        首先,我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下,然后修改CMakeLists.txt文件,将下列代码加入文件底部:
[plain]  view plain  copy
  1. rosbuild_add_boost_directories()  
  2. rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)  
  3. target_link_libraries(smartcar_keyboard_teleop boost_thread)  


        编译完成后,运行smartcar模型。重新打开一个终端,打开键盘控制节点:

        
        在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图:
ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第3张图片

2、复用 

       因为代码是我们直接复制过来的,其中有很多与之前erratic机器人相关的变量,我们把代码稍作修改,变成自己机器人可读性较强的代码。
[cpp]  view plain  copy
  1. #include   
  2. #include   
  3. #include   
  4. #include   
  5. #include   
  6. #include   
  7.   
  8. #include   
  9. #include   
  10. #include   
  11.   
  12. #define KEYCODE_W 0x77  
  13. #define KEYCODE_A 0x61  
  14. #define KEYCODE_S 0x73  
  15. #define KEYCODE_D 0x64  
  16.   
  17. #define KEYCODE_A_CAP 0x41  
  18. #define KEYCODE_D_CAP 0x44  
  19. #define KEYCODE_S_CAP 0x53  
  20. #define KEYCODE_W_CAP 0x57  
  21.   
  22. class SmartCarKeyboardTeleopNode  
  23. {  
  24.     private:  
  25.         double walk_vel_;  
  26.         double run_vel_;  
  27.         double yaw_rate_;  
  28.         double yaw_rate_run_;  
  29.           
  30.         geometry_msgs::Twist cmdvel_;  
  31.         ros::NodeHandle n_;  
  32.         ros::Publisher pub_;  
  33.   
  34.     public:  
  35.         SmartCarKeyboardTeleopNode()  
  36.         {  
  37.             pub_ = n_.advertise("cmd_vel", 1);  
  38.               
  39.             ros::NodeHandle n_private("~");  
  40.             n_private.param("walk_vel", walk_vel_, 0.5);  
  41.             n_private.param("run_vel", run_vel_, 1.0);  
  42.             n_private.param("yaw_rate", yaw_rate_, 1.0);  
  43.             n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);  
  44.         }  
  45.           
  46.         ~SmartCarKeyboardTeleopNode() { }  
  47.         void keyboardLoop();  
  48.           
  49.         void stopRobot()  
  50.         {  
  51.             cmdvel_.linear.x = 0.0;  
  52.             cmdvel_.angular.z = 0.0;  
  53.             pub_.publish(cmdvel_);  
  54.         }  
  55. };  
  56.   
  57. SmartCarKeyboardTeleopNode* tbk;  
  58. int kfd = 0;  
  59. struct termios cooked, raw;  
  60. bool done;  
  61.   
  62. int main(int argc, char** argv)  
  63. {  
  64.     ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);  
  65.     SmartCarKeyboardTeleopNode tbk;  
  66.       
  67.     boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));  
  68.       
  69.     ros::spin();  
  70.       
  71.     t.interrupt();  
  72.     t.join();  
  73.     tbk.stopRobot();  
  74.     tcsetattr(kfd, TCSANOW, &cooked);  
  75.       
  76.     return(0);  
  77. }  
  78.   
  79. void SmartCarKeyboardTeleopNode::keyboardLoop()  
  80. {  
  81.     char c;  
  82.     double max_tv = walk_vel_;  
  83.     double max_rv = yaw_rate_;  
  84.     bool dirty = false;  
  85.     int speed = 0;  
  86.     int turn = 0;  
  87.       
  88.     // get the console in raw mode  
  89.     tcgetattr(kfd, &cooked);  
  90.     memcpy(&raw, &cooked, sizeof(struct termios));  
  91.     raw.c_lflag &=~ (ICANON | ECHO);  
  92.     raw.c_cc[VEOL] = 1;  
  93.     raw.c_cc[VEOF] = 2;  
  94.     tcsetattr(kfd, TCSANOW, &raw);  
  95.       
  96.     puts("Reading from keyboard");  
  97.     puts("Use WASD keys to control the robot");  
  98.     puts("Press Shift to move faster");  
  99.       
  100.     struct pollfd ufd;  
  101.     ufd.fd = kfd;  
  102.     ufd.events = POLLIN;  
  103.       
  104.     for(;;)  
  105.     {  
  106.         boost::this_thread::interruption_point();  
  107.           
  108.         // get the next event from the keyboard  
  109.         int num;  
  110.           
  111.         if ((num = poll(&ufd, 1, 250)) < 0)  
  112.         {  
  113.             perror("poll():");  
  114.             return;  
  115.         }  
  116.         else if(num > 0)  
  117.         {  
  118.             if(read(kfd, &c, 1) < 0)  
  119.             {  
  120.                 perror("read():");  
  121.                 return;  
  122.             }  
  123.         }  
  124.         else  
  125.         {  
  126.             if (dirty == true)  
  127.             {  
  128.                 stopRobot();  
  129.                 dirty = false;  
  130.             }  
  131.               
  132.             continue;  
  133.         }  
  134.           
  135.         switch(c)  
  136.         {  
  137.             case KEYCODE_W:  
  138.                 max_tv = walk_vel_;  
  139.                 speed = 1;  
  140.                 turn = 0;  
  141.                 dirty = true;  
  142.                 break;  
  143.             case KEYCODE_S:  
  144.                 max_tv = walk_vel_;  
  145.                 speed = -1;  
  146.                 turn = 0;  
  147.                 dirty = true;  
  148.                 break;  
  149.             case KEYCODE_A:  
  150.                 max_rv = yaw_rate_;  
  151.                 speed = 0;  
  152.                 turn = 1;  
  153.                 dirty = true;  
  154.                 break;  
  155.             case KEYCODE_D:  
  156.                 max_rv = yaw_rate_;  
  157.                 speed = 0;  
  158.                 turn = -1;  
  159.                 dirty = true;  
  160.                 break;  
  161.                   
  162.             case KEYCODE_W_CAP:  
  163.                 max_tv = run_vel_;  
  164.                 speed = 1;  
  165.                 turn = 0;  
  166.                 dirty = true;  
  167.                 break;  
  168.             case KEYCODE_S_CAP:  
  169.                 max_tv = run_vel_;  
  170.                 speed = -1;  
  171.                 turn = 0;  
  172.                 dirty = true;  
  173.                 break;  
  174.             case KEYCODE_A_CAP:  
  175.                 max_rv = yaw_rate_run_;  
  176.                 speed = 0;  
  177.                 turn = 1;  
  178.                 dirty = true;  
  179.                 break;  
  180.             case KEYCODE_D_CAP:  
  181.                 max_rv = yaw_rate_run_;  
  182.                 speed = 0;  
  183.                 turn = -1;  
  184.                 dirty = true;  
  185.                 break;                
  186.             default:  
  187.                 max_tv = walk_vel_;  
  188.                 max_rv = yaw_rate_;  
  189.                 speed = 0;  
  190.                 turn = 0;  
  191.                 dirty = false;  
  192.         }  
  193.         cmdvel_.linear.x = speed * max_tv;  
  194.         cmdvel_.angular.z = turn * max_rv;  
  195.         pub_.publish(cmdvel_);  
  196.     }  
  197. }  


参考链接:http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation
                  http://www.ros.org/wiki/simulator_gazebo/Tutorials/TeleopErraticSimulation

3、创新

        虽然很多机器人的键盘控制使用的都是C++编写的代码,但是考虑到python的强大,我们还是需要尝试使用python来编写程序。
        首先需要理解上面C++程序的流程。在上面的程序中,我们单独创建了一个线程来读取中断中的输入,然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱,在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符,通过上网查找资料,发现使用的API和C++的基本是一致的。最终的程序如下:
[python]  view plain  copy
  1. #!/usr/bin/env python  
  2. # -*- coding: utf-8 -*  
  3.   
  4. import  os  
  5. import  sys  
  6. import  tty, termios  
  7. import roslib; roslib.load_manifest('smartcar_teleop')  
  8. import rospy  
  9. from geometry_msgs.msg import Twist  
  10. from std_msgs.msg import String  
  11.   
  12. # 全局变量  
  13. cmd = Twist()  
  14. pub = rospy.Publisher('cmd_vel', Twist)  
  15.   
  16. def keyboardLoop():  
  17.     #初始化  
  18.     rospy.init_node('smartcar_teleop')  
  19.     rate = rospy.Rate(rospy.get_param('~hz'1))  
  20.   
  21.     #速度变量  
  22.     walk_vel_ = rospy.get_param('walk_vel'0.5)  
  23.     run_vel_ = rospy.get_param('run_vel'1.0)  
  24.     yaw_rate_ = rospy.get_param('yaw_rate'1.0)  
  25.     yaw_rate_run_ = rospy.get_param('yaw_rate_run'1.5)  
  26.   
  27.     max_tv = walk_vel_  
  28.     max_rv = yaw_rate_  
  29.   
  30.     #显示提示信息  
  31.     print "Reading from keyboard"  
  32.     print "Use WASD keys to control the robot"  
  33.     print "Press Caps to move faster"  
  34.     print "Press q to quit"  
  35.   
  36.     #读取按键循环  
  37.     while not rospy.is_shutdown():  
  38.         fd = sys.stdin.fileno()  
  39.         old_settings = termios.tcgetattr(fd)  
  40.         #不产生回显效果  
  41.         old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO  
  42.         try :  
  43.             tty.setraw( fd )  
  44.             ch = sys.stdin.read( 1 )  
  45.         finally :  
  46.             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  
  47.   
  48.         if ch == 'w':  
  49.             max_tv = walk_vel_  
  50.             speed = 1  
  51.             turn = 0  
  52.         elif ch == 's':  
  53.             max_tv = walk_vel_  
  54.             speed = -1  
  55.             turn = 0  
  56.         elif ch == 'a':  
  57.             max_rv = yaw_rate_  
  58.             speed = 0  
  59.             turn = 1  
  60.         elif ch == 'd':  
  61.             max_rv = yaw_rate_  
  62.             speed = 0  
  63.             turn = -1  
  64.         elif ch == 'W':  
  65.             max_tv = run_vel_  
  66.             speed = 1  
  67.             turn = 0  
  68.         elif ch == 'S':  
  69.             max_tv = run_vel_  
  70.             speed = -1  
  71.             turn = 0  
  72.         elif ch == 'A':  
  73.             max_rv = yaw_rate_run_  
  74.             speed = 0  
  75.             turn = 1  
  76.         elif ch == 'D':  
  77.             max_rv = yaw_rate_run_  
  78.             speed = 0  
  79.             turn = -1  
  80.         elif ch == 'q':  
  81.             exit()  
  82.         else:  
  83.             max_tv = walk_vel_  
  84.             max_rv = yaw_rate_  
  85.             speed = 0  
  86.             turn = 0  
  87.   
  88.         #发送消息  
  89.         cmd.linear.x = speed * max_tv;  
  90.         cmd.angular.z = turn * max_rv;  
  91.         pub.publish(cmd)  
  92.         rate.sleep()  
  93.         #停止机器人  
  94.         stop_robot();  
  95.   
  96. def stop_robot():  
  97.     cmd.linear.x = 0.0  
  98.     cmd.angular.z = 0.0  
  99.     pub.publish(cmd)  
  100.   
  101. if __name__ == '__main__':  
  102.     try:  
  103.         keyboardLoop()  
  104.     except rospy.ROSInterruptException:  
  105.         pass  


参考链接:http://blog.csdn.net/marising/article/details/3173848
                  http://nullege.com/codes/search/termios.ICANON

四、节点关系图

ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第4张图片

        代码包我已上传:http://download.csdn.net/detail/hcx25909/5496381
        希望大家一同学习,共同进步!

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




ROS探索总结(九)——操作杆控制


对于移动机器人,键盘的控制往往满足不了我们的需求,以前看好多电影里边都是用一个摇杆来控制机器人的,简直帅爆了,正好我这里有一个操作杆,那就来尝试感受一下。

        操作杆(joystick)控制会更加有操作感,ROS中的很多机器人也带有操作杆的相关代码,只需要简单的移植即可。我们使用的是赛钛客(saitek)的一款操作杆,如下图所示:

ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第5张图片

        使用的移植代码是clearpath_husky机器人中的Python代码。
        参考链接: http://www.ros.org/wiki/joy

一、测试操作杆驱动

        首先将操作杆的接口插入电脑,然后在终端中输入:
[plain]  view plain  copy
  1. ls /dev/input/   


        显示如下:

        其中的js0就代表我们的操作杆。然后测试操作杆的操作是否有效,输入:
[plain]  view plain  copy
  1. sudo jstest /dev/input/js0  


        然后会在终端中显示操作杆的各个控制值的即时值,操作操作杆,如果每个按键和操作都有效,说明操作杆是正常的。最后在ROS中的节点里进行测试。打开joy节点:
[plain]  view plain  copy
  1. rosrun joy joy_node  


        再打开一个窗口,输入下面命令,查看数据:
[plain]  view plain  copy
  1. rostopic echo joy  


        操作操作杆,窗口下面的数据就开始刷新。

二、控制代码

        从上面的测试中,我们发现在前后左右摇动操作杆时,相应改变的数据是Axes中0号和1号位的数据,也是我们最常用的数据,其他按键对应的位置也可以找到,编程的时候就是利用的这些数据位置,所以一定要找到每个按键的对应编号。
        ROS中已经为我们建立了操作杆的数据结构:
ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第6张图片
        我们主要用到的就是axes和buttons数据。最终的代码如下:
[python]  view plain  copy
  1. import roslib; roslib.load_manifest('smartcar_teleop')  
  2. import rospy  
  3.   
  4. from sensor_msgs.msg import Joy  
  5. from geometry_msgs.msg import Twist  
  6. from std_msgs.msg import String  
  7.   
  8. class Teleop:  
  9.     def __init__(self):  
  10.         rospy.init_node('smartcar_teleop_joy')  
  11.   
  12.         self.turn_scale = rospy.get_param('~turn_scale')  
  13.         self.drive_scale = rospy.get_param('~drive_scale')  
  14.         self.deadman_button = rospy.get_param('~deadman_button'0)  
  15.   
  16.         self.cmd = None  
  17.         cmd_pub = rospy.Publisher('cmd_vel', Twist)  
  18.   
  19.         announce_pub = rospy.Publisher('/smartcar/announce/teleops',  
  20.                                        String, latch=True)  
  21.         announce_pub.publish(rospy.get_namespace());  
  22.   
  23.         rospy.Subscriber("joy", Joy, self.callback)  
  24.         rate = rospy.Rate(rospy.get_param('~hz'20))  
  25.           
  26.         while not rospy.is_shutdown():  
  27.             rate.sleep()  
  28.             if self.cmd:  
  29.                 cmd_pub.publish(self.cmd)  
  30.   
  31.     def callback(self, data):  
  32.         """ Receive joystick data, formulate Twist message. """  
  33.         cmd = Twist()  
  34.         cmd.linear.x = data.axes[1] * self.drive_scale  
  35.         cmd.angular.z = data.axes[0] * self.turn_scale  
  36.   
  37.         if data.buttons[self.deadman_button] == 1:  
  38.             self.cmd = cmd  
  39.         else:  
  40.             self.cmd = None  
  41.   
  42. if __name__ == "__main__": Teleop()  


三、机器人控制

        首先来创建一个launch文件(teleop_joy.launch):
[plain]  view plain  copy
  1.   
  2.     
  3.     
  4.     
  5.     
  6.   
  7.     
  8.       
  9.       
  10.     
  11.   
  12.     
  13.       
  14.       
  15.       
  16.     
  17.   


        在rviz中打开我们的机器人模型,然后打开操作杆的控制节点:
[plain]  view plain  copy
  1. roslaunch smartcar_display.rviz.launch  
  2. roslaunch smartcar_teleop teleop_joy.launch  


        然后按住刹车键进行操作,机器人就可以开始移动了:
ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第7张图片
        在新终端中输入:
[plain]  view plain  copy
  1. rostopic echo joy  


         可以查看到实时的操作杆控制数据:
ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第8张图片

四、节点关系图
ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制_第9张图片

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

欢迎大家转载我的文章。

转载请注明:转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客



你可能感兴趣的:(ROS)