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

ROS探索总结（七）——smartcar源码上传

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

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

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

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

 rosmake smartcar_description --pre-clean

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

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

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

 "gui" default="false" />

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

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

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

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

 

PS：       

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

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

ROS探索总结（八）——键盘控制

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

一、创建控制包

        首先，我们为键盘控制单独建立一个包：

 roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
 rosmake

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

二、简单的消息发布

        在机器人仿真中，主要控制机器人移动的就是        在机器人仿真中，主要控制机器人移动的就是Twist()结构，如果我们编程将这个结构通过程序发布成topic，自然就可以控制机器人了。我们先用简单的python来尝试一下。

        之前的模拟中，我们使用的都是在命令行下进行的消息发布，现在我们需要把这些命令转换成python代码，封装到一个单独的节点中去。针对之前的命令行，我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码：

 #!/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()

       python代码在ROS重视不需要编译的。先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：

 rosrun smartcar_teleop teleop.py

        也可以建立一个launch文件运行：

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

三、加入键盘控制

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

1、移植

        因为ROS的代码具有很强的可移植性，所以用键盘控制的代码其实可以直接从其他机器人包中移植过来，在这里我主要参考的是erratic_robot，在这个机器人的代码中有一个erratic_teleop包，可以直接移植过来使用。
        首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：

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

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

        

        在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图：

2、复用 

       因为代码是我们直接复制过来的，其中有很多与之前erratic机器人相关的变量，我们把代码稍作修改，变成自己机器人可读性较强的代码。

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

参考链接：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++的基本是一致的。最终的程序如下：

 #!/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/marising/article/details/3173848
                  http://nullege.com/codes/search/termios.ICANON

四、节点关系图

        代码包我已上传：http://download.csdn.net/detail/hcx25909/5496381

        希望大家一同学习，共同进步！

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

ROS探索总结（九）——操作杆控制

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

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

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

一、测试操作杆驱动

        首先将操作杆的接口插入电脑，然后在终端中输入：

 ls /dev/input/

        显示如下：

        其中的js0就代表我们的操作杆。然后测试操作杆的操作是否有效，输入:

 sudo jstest /dev/input/js0

        然后会在终端中显示操作杆的各个控制值的即时值，操作操作杆，如果每个按键和操作都有效，说明操作杆是正常的。最后在ROS中的节点里进行测试。打开joy节点：

 rosrun joy joy_node

        再打开一个窗口，输入下面命令，查看数据:

 rostopic echo joy

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

二、控制代码

        从上面的测试中，我们发现在前后左右摇动操作杆时，相应改变的数据是Axes中0号和1号位的数据，也是我们最常用的数据，其他按键对应的位置也可以找到，编程的时候就是利用的这些数据位置，所以一定要找到每个按键的对应编号。

        ROS中已经为我们建立了操作杆的数据结构：

        我们主要用到的就是axes和buttons数据。最终的代码如下：

 import roslib; roslib.load_manifest('smartcar_teleop')
 import rospy

 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import Twist
 from std_msgs.msg import String

 class Teleop:
     def __init__(self):
         rospy.init_node('smartcar_teleop_joy')

         self.turn_scale = rospy.get_param('~turn_scale')
         self.drive_scale = rospy.get_param('~drive_scale')
         self.deadman_button = rospy.get_param('~deadman_button', 0)

         self.cmd = None
         cmd_pub = rospy.Publisher('cmd_vel', Twist)

         announce_pub = rospy.Publisher('/smartcar/announce/teleops',
                                        String, latch=True)
         announce_pub.publish(rospy.get_namespace());

         rospy.Subscriber("joy", Joy, self.callback)
         rate = rospy.Rate(rospy.get_param('~hz', 20))

         while not rospy.is_shutdown():
             rate.sleep()
             if self.cmd:
                 cmd_pub.publish(self.cmd)

     def callback(self, data):
         """ Receive joystick data, formulate Twist message. """
         cmd = Twist()
         cmd.linear.x = data.axes[1] * self.drive_scale
         cmd.angular.z = data.axes[0] * self.turn_scale

         if data.buttons[self.deadman_button] == 1:
             self.cmd = cmd
         else:
             self.cmd = None

 if __name__ == "__main__": Teleop()

三、机器人控制

        首先来创建一个launch文件（teleop_joy.launch）:

        在rviz中打开我们的机器人模型，然后打开操作杆的控制节点：

 roslaunch smartcar_display.rviz.launch
 roslaunch smartcar_teleop teleop_joy.launch

        然后按住刹车键进行操作，机器人就可以开始移动了：

        在新终端中输入：

 rostopic echo joy

         可以查看到实时的操作杆控制数据：

四、节点关系图

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

欢迎大家转载我的文章。

转载请注明：转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客