ROS新手教程 前面我们介绍了ROS的特点和结构，接下来就要开始准备动手感受一下ROS的强大了。ROS官网的wiki上针对新手的教程很详细，最好把所有的新手教程都搞清楚，这是后面开发最基础的东西。尽管如此，ROS对于新手来说还是很难上手，这里，我就来总结一下我当时学习的历程，也为其他新手作为一个参考。 一、ROS的安装 ROS的安装当然是我们开始动手的第一步了，这里我们使用的操作系统是ubuntu，因为ROS在ubuntu上的支持是最好的。 如果是新手，我建议使用”apt-get“的方法进行安装，不走很简单，按照wiki上说的，大概半个小时就可以安装完毕完全版的ROS：（现在最新版的ROS是groovy，但是我还是习惯使用fuerte） http://ros.org/wiki/fuerte/Installation/Ubuntu 如果想挑战源码编译，当然也没有问题： http://ros.org/wiki/fuerte/Installation/Ubuntu/Source 安装完毕之后运行一下“roscore”，如果没有问题，安装就成功了！ 注：groovy版本的安装： “apt-get”：http://www.ros.org/wiki/groovy/Installation/Ubuntu 源码编译：http://www.ros.org/wiki/groovy/Installation/Source 转载注： 安装部分的介绍已经比较旧, 参考稳定版本indigo http://wiki.ros.org/indigo/Installation 二、ROS的新手教程 wiki上的新手教程还是很详细的，对代码都有解释，新手一定要把这些例子和代码搞明白： http://www.ros.org/wiki/ROS/Tutorials 上面的教程都是英文的，如果感觉略有压力（本人就是），可以参考下面这两个博客中的部分翻译： http://blog.sina.com.cn/s/articlelist_1712413141_0_1.html 不过往后面的学习都是英文的资料了，还是要努力适应看英文的文档。 ROS使用的编程语言主要是C++和python，所以也有针对这两种语言的功能包roscpp和rospy，这两个包的教程与上面的教程基本相似，看完上面的教程也可以看看这两个包的教程： http://www.ros.org/wiki/rospy_tutorials http://www.ros.org/wiki/roscpp/Tutorials 努力学习完上面的这些教程，你至少应该明白ROS里面的节点和消息是干什么用的了吧，如果还没理解，那就再多看几遍吧！ 三、ROS中的常用功能 ROS中提供了很多强大的功能，我们学习完上面的基本知识之后要继续进行深入。 1、rviz rviz是ROS中一款强大的3D可视化工具，这个玩意在后面可是要频繁用到的，是必须要弄明白的，详细的教程可以参考wiki： http://www.ros.org/wiki/rviz 我们可以在里面创建自己的机器人，并且让机器人动起来。还可以创建地图，显示3D点云等等，总之，想在ROS中显示的东东都可以在这里显示出来。当然这些显示都是通过消息的订阅来完成的，机器人通过ROS发布数据，rviz订阅消息接收数据，然后显示，这些数据也是有一定的数据格式的，可以参考下面的链接： http://www.ros.org/wiki/rviz/DisplayTypes 看到上面的机器人了吧，是不是很酷，在rviz中，这样的机器人模型是通过urdf文件描述的，具体urdf文件怎么写，参考wiki： http://www.ros.org/wiki/urdf 2、tf tf是ROS中的坐标变换系统，在机器人的建模仿真中经常用到。 ROS中主要有两种坐标系： （1）固定坐标系：用于表示世界的参考坐标系; （2）目标坐标系：相对于摄像机视角的参考坐标系。 教程见：http://www.ros.org/wiki/tf 3、gazebo 这个工具是ROS中的物理仿真环境，gazebo本身就是一款机器人的仿真软件，基于ODE的物理引擎，可以模拟机器人以及环境中的很多物理特性，这个软件可以稍作了解，并不是后面开发所必须要的。 教程见：http://www.ros.org/wiki/simulator_gazebo/Tutorials 四、ROS常用机器人 1、PR2 看ROS的应用时，最常见到的机器人就是PR2。这个机器人是ROS的主要维护者（Willow Garage）针对ROS量身定做的机器人，有两个运行ubuntu和ROS的电脑，和两个机器臂以及很多牛逼的传感器，功能是非常强大的，但是售价那是相当的昂贵，国内很少见到，基本都是在国外的研究所里。这款机器人的ROS包比较多，从仿真到导航，所以代码具有比较高的参考价值，当然新手还是先看看其他机器人的代码再来挑战PR2吧，代码比较庞杂。 wiki：http://www.ros.org/wiki/Robots/PR2 2、TurtleBot 这个机器人应该算是应用ROS小型移动机器人的典型代表了，资料、文档和代码比较多，主要在建立模型和导航定位方面，代码比较 wiki：http://www.ros.org/wiki/Robots/TurtleBot 3、Husky、Erratic 这两款机器人和TurtleBot机器人差不多，都是小型的轮式移动机器人，同样可以作为新手学习的参考： http://www.ros.org/wiki/Robots/Husky http://www.ros.org/wiki/Robots/Erratic 这些教程和机器人基本都是我当时学习的时候研究和了解过的，总结在此也让那些比较迷茫的初学者作为参考，可以尽快了解和掌握ROS。再次强调一下，wiki上的新手教程是一定要先熟悉的，后面的内容在后续学习过程中可以一边做一边学习。
简单的机器人仿真 修改版：2016-10-13 by ncnynl 前边我们已经介绍了ROS的基本情况，以及新手入门ROS的初级教程，现在就要真正的使用ROS进入机器人世界了。接下来我们涉及到的很多例程都是《ROS by Example》这本书的内容，我是和群里的几个人一起从国外的亚马逊上买到的，还是很有参考价值的，不过前提是你已经熟悉之前的新手教程了。 《ROS by Example》目前是第二版《ROS by Example v1 indigo》 可点击加入Q群:477659623，进入群文件下载。 本章部分内容来自《ROS by Example v1 indigo》，请按书中的说明安装例子。 一、ROS by Example 这本书是关于国外关于ROS出版的第一本书，主要针对Electric和Fuerte版本，使用机器人主要是TurtleBot。书中详细讲解了关于机器人的基本仿真、导航、路径规划、图像处理、语音识别等等，而且在google的svn上发布了所有代码，可以通过以下命令下载、编译： svn checkout http://ros-by-example.googlecode.com/svn/trunk/rbx_vol_1 rosmake rbx_vol_1 rospack profile //加入ROS package路径 注意：原来的内容已经无效，更改为针对indigo版本： 检查环境并安装依赖 $ cd ~ $ wget https://raw.githubusercontent.com/pirobot/rbx1/indigo-devel/rbx1-prereq.sh $ sh rbx1-prereq.sh 安装书中例子 $ cd ~/catkin_ws/src/ $ git clone https://github.com/pirobot/rbx1.git $ cd rbx1 $ git checkout indigo-devel $ cd ~/catkin_ws $ catkin_make $ source ~/catkin_ws/devel/setup.bash $ rospack profile 二、rviz简单机器人模拟 1、安装机器人模拟器 rviz是一个显示机器人实体的工具，本身不具有模拟的功能，需要安装一个模拟器arbotix。 svn checkout http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix rosmake arbotix 注意：原来内容已经无效，更改为indigo版本： $sudo apt-get install ros-indigo-arbotix-* $rospack profile 2、TurtleBot机器人的模拟 在书中的rbx_vol_1包里已经为我们写好了模拟的代码，我们先进行实验，完成后再仔细研究代码。 机器人模拟运行： 在新终端打开： $ roscore 新终端打开： $ roslaunch rbx1_bringup fake_pi_robot.launch 然后在终端中可以看到，机器人已经开始运行了，打开rviz界面，才能看到机器人实体。 rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz 注意：适合indigo版本原来为sim_fuerte.vcg，更改为sim.rviz 后面的参数是加载了rviz的配置文件sim.rviz。效果如下： 此时的机器人是静止的，需要发布一个消息才能让它动起来。 rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}' 如果要让机器人停下来，需要在中断中按下“Ctrl+c”，然后输入： rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}' 也可以改变发送的topic信息，使机器人走出不同的轨迹。 三、实现分析 按照上面的仿真过程，我们详细分析每一步的代码实现。 1、TurtleBot机器人运行 机器人运行使用的是launch文件，首先打开fake_turtlebot.launch文件。 注意：请比较书中的indigo版本，此launch已经旧了 <launch> <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" /> <node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen"> <rosparam file="$(find rbx1_bringup)/config/fake_turtlebot_arbotix.yaml" command="load" /> </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"> </node> <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_wheel_link 100" /> <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_wheel_link 100" /> </launch> 上面版本已经旧新的indigo版本为： <launch> <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find rbx1_description)/urdf/turtlebot.urdf.xacro'" /> <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true"> <rosparam file="$(find rbx1_bringup)/config/fake_turtlebot_arbotix.yaml" command="load" /> </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"> </node> </launch> 文件可以大概分为四个部分： （1） 从指定的包中加载urdf文件 （2） 启动arbotix模拟器 （3） 启动状态发布节点 （4） tf坐标系配置 2、rviz配置文件 在打开rviz的时候需要加载一个.vcg的配置文件，主要对rviz中的插件选项进行默认的配置。这里打开的是sim_fuerte.vcg文件，由于文件比较长，这里只列举重点的部分。 Background\ ColorB=0.12549 Background\ ColorG=0.12549 Background\ ColorR=0.12549 Camera\ Config=158.108 0.814789 0.619682 -1.57034 Camera\ Type=rviz::FixedOrientationOrthoViewController Fixed\ Frame=/odom Grid.Alpha=0.5 Grid.Cell\ Size=0.5 Grid.ColorB=0.941176 Grid.ColorG=0.941176 Grid.ColorR=0.941176 Grid.Enabled=1 Grid.Line\ Style=0 Grid.Line\ Width=0.03 Grid.Normal\ Cell\ Count=0 Grid.OffsetX=0 Grid.OffsetY=0 Grid.OffsetZ=0 Grid.Plane=0 注意：sim_fuerte.vcg为旧版本，新版本为sim.rviz, 打开路径为：rbx1_nav/sim.rviz 上面的代码是配置背景颜色和网格属性的，对应rviz中的选项如下图所示。 其中比较重要的一个选项是Camera的type，这个选项是控制开发者的观察角度的，书中用的是FixedOrientationOrthoViewController的方式，就是上面图中的俯视角度，无法看到机器人的三维全景，所以可以改为OrbitViewController方式，如下图所示： 3、发布topic 要让机器人动起来，还需要给他一些运动需要的信息，这些信息都是通过topic的方式发布的。 这里的topic就是速度命令，针对这个topic，我们需要发布速度的信息，在ROS中已经为我们写好了一些可用的数据结构，这里用的是Twist信息的数据结构。在终端中可以看到Twist的结构如下： 用下面的命令进行消息的发布，其中主要包括力的大小和方向。 [plain] view plain copy Background\ ColorB=0.12549 Background\ ColorG=0.12549 Background\ ColorR=0.12549 Camera\ Config=158.108 0.814789 0.619682 -1.57034 Camera\ Type=rviz::FixedOrientationOrthoViewController Fixed\ Frame=/odom Grid.Alpha=0.5 Grid.Cell\ Size=0.5 Grid.ColorB=0.941176 Grid.ColorG=0.941176 Grid.ColorR=0.941176 Grid.Enabled=1 Grid.Line\ Style=0 Grid.Line\ Width=0.03 Grid.Normal\ Cell\ Count=0 Grid.OffsetX=0 Grid.OffsetY=0 Grid.OffsetZ=0 Grid.Plane=0 4、节点关系图
创建简单的机器人模型smartcar 修改版：2016-10-13 by ncnynl 前面我们使用的是已有的机器人模型进行仿真，这一节我们将建立一个简单的智能车机器人smartcar，为后面建立复杂机器人打下基础。 urdf相关工具安装（补充） 安装urdf_tutorial，可以使用其中的例子 $ sudo apt-get install ros-indigo-urdf-tutorial 安装liburdfdom-tools，提供对urdf文件的检查 $ sudo apt-get install liburdfdom-tools 一、创建硬件描述包 $ roscreate-pkg smartcar_description 注意：上面方法旧了，indigo版本使用catkin_create_pkg创建包： $ catkin_create_pkg smartcar_description std_msgs rospy roscpp urdf 二、智能车尺寸数据 因为建立的是一个非常简单的机器人，所以我们尽量使用简单的元素：使用长方体代替车模，使用圆柱代替车轮，具体尺寸如下： 三、建立urdf文件 在smartcar_description文件夹下建立urdf文件夹 $mkdir -p urdf ，创建智能车的描述文件smartcar.urdf，描述代码如下： <?xml version="1.0"?> <robot name="smartcar"> <visual> <geometry> <box size="0.25 .16 .05"/> </geometry> <origin rpy="0 0 1.57075" xyz="0 0 0"/> <material name="blue"> <color rgba="0 0 .8 1"/> </material> </visual> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <joint name="right_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_front_wheel"/> <origin rpy="0 1.57075 0" xyz="0.08 0.1 -0.03"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <joint name="right_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_back_wheel"/> <origin rpy="0 1.57075 0" xyz="0.08 -0.1 -0.03"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <joint name="left_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_front_wheel"/> <origin rpy="0 1.57075 0" xyz="-0.08 0.1 -0.03"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <joint name="left_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_back_wheel"/> <origin rpy="0 1.57075 0" xyz="-0.08 -0.1 -0.03"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <visual> <geometry> <box size=".02 .03 .03"/> </geometry> <material name="white"> <color rgba="1 1 1 1"/> </material> </visual> <joint name="tobox" type="fixed"> <parent link="base_link"/> <child link="head"/> <origin xyz="0 0.08 0.025"/> </joint> </robot> 四、建立launch命令文件 在smartcar_description文件夹下建立launch文件夹mkdir -p launch，创建智能车的描述文件 base.urdf.rviz.launch，描述代码如下： <launch> <arg name="model" /> <arg name="gui" default="False" /> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" > </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" /> </launch> 注意：上面的launch已经旧了，indigo版本launch如下： <launch> <arg name="model" /> <arg name="gui" default="False" /> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" > </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" /> </launch> 五、效果演示 在终端中输入显示命令： roslaunch smartcar_description base.urdf.rviz.launch gui:=true 显示效果如下图所示，使用gui中的控制bar可以控制四个轮子单独旋转。
使用smartcar进行仿真 之前的博客中，我们使用rviz进行了TurtleBot的仿真，而且使用urdf文件建立了自己的机器人smartcar，本篇博客是将两者进行结合，使用smartcar机器人在rviz中进行仿真。 urdf相关工具安装（补充） 安装urdf_tutorial，可以使用其中的例子 $ sudo apt-get install ros-indigo-urdf-tutorial 安装liburdfdom-tools，提供对urdf文件的检查 $ sudo apt-get install liburdfdom-tools 一、模型完善 之前我们使用的都是urdf文件格式的模型，在很多情况下，ROS对urdf文件的支持并不是很好，使用宏定义的.xacro文件兼容性更好，扩展性也更好。所以我们把之前的urdf文件重新整理编写成.xacro文件。 .xacro文件主要分为三部分： 1、机器人主体smartcar_body.urdf.xacro <?xml version="1.0"?> <robot name="smartcar" xmlns:xacro="http://ros.org/wiki/xacro"> <property name="M_PI" value="3.14159"/> <include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/> <property name="base_x" value="0.33" /> <property name="base_y" value="0.33" /> <xacro:macro name="smartcar_body"> <inertial> <origin xyz="0 0 0.055"/> <mass value="1.0" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <box size="0.25 .16 .05"/> </geometry> <origin rpy="0 0 0" xyz="0 0 0.055"/> <material name="blue"> <color rgba="0 0 .8 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0.055"/> <geometry> <box size="0.25 .16 .05" /> </geometry> </collision> <inertial> <origin xyz="0.08 0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="left_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_front_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="0.08 -0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="right_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_front_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="-0.08 0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="left_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_back_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="-0.08 -0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="right_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_back_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="0.08 0 0.08"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <box size=".02 .03 .03"/> </geometry> <material name="white"> <color rgba="1 1 1 1"/> </material> </visual> <collision> <origin xyz="0.08 0 0.08"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="tobox" type="fixed"> <parent link="base_link"/> <child link="head"/> <origin xyz="0.08 0 0.08"/> </joint> </xacro:macro> </robot> 注意：原版本在indigo可能运行有问题，修改的indigo版本如下： <?xml version="1.0"?> <robot name="smartcar" xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:property name="M_PI" value="3.14159"/> <xacro:include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/> <xacro:property name="base_x" value="0.33" /> <xacro:property name="base_y" value="0.33" /> <xacro:macro name="smartcar_body"> <inertial> <origin xyz="0 0 0.055"/> <mass value="1.0" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <box size="0.25 .16 .05"/> </geometry> <origin rpy="0 0 0" xyz="0 0 0.055"/> <material name="blue"> <color rgba="0 0 .8 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0.055"/> <geometry> <box size="0.25 .16 .05" /> </geometry> </collision> <inertial> <origin xyz="0.08 0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="left_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_front_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="0.08 -0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="right_front_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_front_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="-0.08 0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="left_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="left_back_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="-0.08 -0.08 0.025"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> <material name="black"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="right_back_wheel_joint" type="continuous"> <axis xyz="0 0 1"/> <parent link="base_link"/> <child link="right_back_wheel"/> <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/> <limit effort="100" velocity="100"/> <joint_properties damping="0.0" friction="0.0"/> </joint> <inertial> <origin xyz="0.08 0 0.08"/> <mass value="0.1" /> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> </inertial> <visual> <geometry> <box size=".02 .03 .03"/> </geometry> <material name="white"> <color rgba="1 1 1 1"/> </material> </visual> <collision> <origin xyz="0.08 0 0.08"/> <geometry> <cylinder length=".02" radius="0.025"/> </geometry> </collision> <joint name="tobox" type="fixed"> <parent link="base_link"/> <child link="head"/> <origin xyz="0.08 0 0.08"/> </joint> </xacro:macro> </robot> 修改property，为xacro:property 修改include， 为xacro:include 2、gazebo属性部分 gazebo.urdf.xacro <?xml version="1.0"?> <robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro" name="smartcar_gazebo"> <xacro:macro name="smartcar_sim"> <gazebo reference="base_link"> <material>Gazebo/Blue</material> </gazebo> <gazebo reference="right_front_wheel"> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="right_back_wheel"> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="left_front_wheel"> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="left_back_wheel"> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="head"> <material>Gazebo/White</material> </gazebo> </xacro:macro> </robot> 3、主文件 smartcar.urdf.xacro <?xml version="1.0"?> <robot name="smartcar" xmlns:xi="http://www.w3.org/2001/XInclude" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:xacro="http://ros.org/wiki/xacro"> <include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" /> <smartcar_body/> <smartcar_sim/> </robot> 注意：原来的在indigo下运行有问题，修改的indigo版本如下： <?xml version="1.0"?> <robot name="smartcar" xmlns:xi="http://www.w3.org/2001/XInclude" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" /> <smartcar_body/> <smartcar_sim/> </robot> 修改include， 为xacro:include 二、lanuch文件smartcar_display.rviz.launch 在launch文件中要启动节点和模拟器。 <launch> <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" /> <arg name="gui" default="false" /> <node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen"> <rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" /> </node> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" > </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"> </node> <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" /> <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.vcg" /> </launch> 注意：原来的版本在indigo运行有问题，修改的indigo版本如下： <launch> <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" /> <arg name="gui" default="false" /> <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen"> <rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" /> </node> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" > </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"> </node> <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" /> <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.rviz" /> </launch> 修改driver.py为arbotix_driver 修改urdf.vcg 为urdf.rviz,确保在smartcar_description目录下有urdf.rviz。从urdf_tutorial目录提取 $ cd ~/catkin_ws/src/smartcar_description $ cp /opt/ros/indigo/share/urdf_tutorial/urdf.rviz . 在indigo版本里，新增smartcar_description/config/smartcar_arbotix.yaml文件，内容为： port: /dev/ttyUSB0 baud: 115200 rate: 20 sync_write: True sync_read: True read_rate: 20 write_rate: 20 controllers: { # Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100) base_controller: {type: diff_controller, base_frame_id: base_link, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 } } 三、仿真测试 首先运行lanuch，既可以看到rviz中的机器人： roslaunch smartcar_description smartcar_display.rviz.launch 效果图： 发布一条动作的消息： rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}' 效果图： 四、节点关系 运行rqt_graph： $ rqt_graph 效果图： 参考： http://wiki.ros.org/urdf/Tutorials http://wiki.ros.org/xacro http://blog.csdn.net/sunbibei/article/details/52297524 http://blog.csdn.net/sujun3304/article/details/18962719
smartcar源码上传 修改版：2016-10-13 by ncnynl 看到前面写的博客还是帮助了很多ROS的学习者，我感到非常荣幸。其实我也是一名ROS的新手，ROS的相关资料少，上手难度大，我现在也在摸索着学习，还希望大家都将自己的学习成果在网上或者ROS群里分享。 我看到有些人在运行我前面写的smartcar程序，为了方便大家的学习，我这两天整理了一下代码，已经上传到csdn上，下载请见： http://download.csdn.net/detail/hcx25909/5487985 或者 http://file.ncnynl.com/ros/smartcar.zip 下载 注意：修改的indigo版本： http://file.ncnynl.com/ros/smartcar_description_indigo_ncnynl.com.tar.gz 下载 注意事项 代码的内容主要是前边用到的机器人smartcar的urdf文件，已经运行使用的launch文件，大家下载后可以直接编译使用，由于代码在我的计算机上编译时带有路径信息，所以大家编译的时候请使用下面的命令先清除再编译： rosmake smartcar_description --pre-clean 注意：indigo版本用catkin编译： $ cd ~/catkin_ws/ $ catkin_make 代码中的urdf文件里室机器人的urdf描述，和上一篇博客中讲的是一样的，launch文件夹中主要有以下文件： 第一个实在gazebo仿真中用到的，需要先运行gazebo，然后再运行。但是貌似有问题，我每次加载到gazebo中的时候似乎物理参数不对，机器人在gazebo里飞来飞去的，至今还没有解决。如果哪位仁兄发现问题了，还请多多指教。 第二个文件是我在绘制模型的时候用到的，主要功能是在rviz中显示机器人模型，文件中的以下代码： <arg name="gui" default="false" /> 如果把false改为true，则会打开一个gui的调试界面，可以通过滑动条旋转车轮，如下图所示。默认是false。 第三个文件是在方针的时候用到的，就是前面博客中使用的，会打开arbotix仿真器。 根目录中还有rviz的配置文件urdf.vcg，只要大家在rviz中修改界面或者参数，推出的时候保存，就会自动修改这个文件了。 config文件夹下的是仿真用到的配置文件。 PS：代码中还有很多问题，请大家多多指教，共同进步。
键盘控制 如果尝试过前面的例子，有没有感觉每次让机器人移动还要在终端里输入指令，这也太麻烦了，有没有办法通过键盘来控制机器人的移动呢？答案室当然的了。我研究了其他几个机器人键盘控制的代码，还是有所收获的，最后移植到了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文件运行： <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> 在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 <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)) < 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 或 http://file.ncnynl.com/ros/smartcar_teleop.zip 下载 希望大家一同学习，共同进步！
操作杆控制 对于移动机器人，键盘的控制往往满足不了我们的需求，以前看好多电影里边都是用一个摇杆来控制机器人的，简直帅爆了，正好我这里有一个操作杆，那就来尝试感受一下。 操作杆（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）: <launch> <arg name="drive_speed" default="1.0" /> <arg name="turn_speed" default="1.0" /> <arg name="joy_dev" default="/dev/input/js0" /> <arg name="cmd_topic" default="cmd_vel" /> <node pkg="joy" type="joy_node" name="joy_node"> </node> <node pkg="smartcar_teleop" type="teleop_joy.py" name="smartcar_teleop"> <remap from="cmd_vel" to="$(arg cmd_topic)" /> </node> </launch> 在rviz中打开我们的机器人模型，然后打开操作杆的控制节点： roslaunch smartcar_display.rviz.launch roslaunch smartcar_teleop teleop_joy.launch 然后按住刹车键进行操作，机器人就可以开始移动了： 在新终端中输入： rostopic echo joy 可以查看到实时的操作杆控制数据： 四、节点关系图
机器视觉 机器视觉在计算机时代已经越来越流行，摄像头价格越来越低廉，部分集成深度传感器的混合型传感器也逐渐在研究领域普及，例如微软推出的Kinect，而且与之配套的软件功能十分强大，为开发带来了极大的便利。ROS集成了Kinect的的驱动包OpenNI，而且使用OpenCV库可以进行多种多样的图像处理。 注：本章内容及代码均参考《ROS by Example》书中的第十章。 一、图像显示 我们从最基础的开始，想办法显示Kinect的图像数据。 1、安装驱动包 安装步骤很简单： $sudo apt-get install ros-fuerte-openni-kinect 2、测试 首先运行kinect节点： $roslaunch rbx1_vision openni_node_fuerte.launch 然后我们调用ROS的image_view包来直接显示摄像头的数据库。 image_view包的介绍可以参考：http://www.ros.org/wiki/image_view。 $rosrun image_view image_view image:=/camera/rgb/image_color 我们可以看到弹出了一个独立的图像显示框： 3、分析数据 下图是我们上面测试中的节点图。 我们可以使用如下的命令来查看节点之间发送的图像消息是什么样的： rostopic echo /camera/rgb/image_color 然后就会看到数据刷刷的在显示，有没有感觉看不清楚，我们使用终端的重定向功能将数据全部存储到文件中： rostopic echo /camera/rgb/image_color > test 好了，现在看看文件中是不是已经有数据了，我们来看一下其中的一帧数据： header: seq: 19285 stamp: secs: 1370867560 nsecs: 538447820 frame_id: camera_rgb_optical_frame height: 240 width: 320 encoding: rgb8 is_bigendian: 0 step: 960 data: [223, 225, 225, 220, 225, 225…………….. 从数据中我们可以的出来几个很重要的参数，首先是图像的分辨率：240320，编码的格式是rgb8，也就是说图像应该有240320=76800个像素，而每个像素由八位的R、G、B三个数据组成，因此我们可以预计下面的data中应该有76800*3=230400个八位的数据了。 我们可以验证一下data中到底有多少个数据，这个很简单了，数一下就行了，不过好像有点多，我使用的是linux的“wc”命令。首先我一帧数据复制到单独的文件中，每个数据都是用“，”号隔开的，只要计算“，”的数量就知道数据的数量了。 结果和我们预想的是一样的。知道这个数据格式以后，我们以后就可以直接把其他摄像头的数据装换成这种格式的数据就可以直接在ROS中使用了。 4、rviz显示图像 我们再来点高级的。rviz是我们经常使用的工具，把图像显示在rviz中才更有应用价值。rviz已经为我们提供好了显示图像的接口，使用非常简单。 首先按照上一节的方法运行kinect节点，然后打开rviz： rosrun rviz rviz 然后修改“Fixed Frame”为/camera_rgb_color，修改“Target Frame”为，接着点击add，选择camera类型。添加成功后选择camera菜单下的Iamge Topic选项，选择/camera/rgb/image_color，确定后下面的显示框内就显示图像了。 二、深度显示 使用kinect的一大特色就是可以获得传感器的深度数据，也就是物体距离传感器的距离，传说kinect的可识别范围在60cm到10m之间。 1、显示深度图像 首先也需要运行Kinect的节点： roslaunch openni_launch openni.launch 这一步我使用的是ROS安装openni包中的节点，使用与前面相同的命令运行的节点在后面无法产生深度数据。 然后同样适用iamge_view包就可以简单的显示深度图像了： rosrun image_view disparity_view image:=/camera/depth/disparity 2、rviz中显示深度 首先运行rviz： rosrun rviz rviz 然后修改“Fixed Frame”和“Target Frame”，在add中添加PointCloud2类型，修改“topic”，具体参数如下图所示：
坐标系统 在机器人的控制中，坐标系统是非常重要的，在ROS使用tf软件库进行坐标转换。 相关链接：http://www.ros.org/wiki/tf/Tutorials#Learning_tf 一、tf简介 我们通过一个小小的实例来介绍tf的作用。 1、安装turtle包 $ rosdep install turtle_tf rviz $ rosmake turtle_tf rviz 2、运行demo 运行简单的demo： $ roslaunch turtle_tf turtle_tf_demo.launch 然后就会看到两只小乌龟了。 该例程中带有turtlesim仿真，可以在终端激活的情况下进行键盘控制。 可以发现，第二只乌龟会跟随你移动的乌龟进行移动。 3、demo分析 接下来我们就来看一看到底ROS做了什么事情。 这个例程使用tf建立了三个参考系：a world frame, a turtle1 frame, and a turtle2 frame。然后使用tf broadcaster发布乌龟的参考系，并且使用tf listener计算乌龟参考系之间的差异，使得第二只乌龟跟随第一只乌龟。 我们可以使用tf工具来具体研究。 $ rosrun tf view_frames 然后会看到一些提示，并且生成了一个frames.pdf文件。 该文件描述了参考系之间的联系。三个节点分别是三个参考系，而/world是其他两个乌龟参考系的父参考系。还包含一些调试需要的发送频率、最近时间等信息。 tf还提供了一个tf_echo工具来查看两个广播参考系之间的关系。我们可以看一下第二只得乌龟坐标是怎么根据第一只乌龟得出来的。 $ rosrun tf tf_echo turtle1 turtle2 控制一只乌龟，在终端中会看到第二只乌龟的坐标转换关系。 我们也可以通过rviz的图形界面更加形象的看到这三者之间的关系。 $ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.vcg 移动乌龟，可以看到在rviz中的坐标会跟随变化。其中左下角的是/world，其他两个是乌龟的参考系。 下面我们就来详细分析这个实例。 二、Writing a tf broadcaster 1、创建包 $ roscd tutorials $ roscreate-pkg learning_tf tf roscpp rospy turtlesim $ rosmake learning_tf 2、broadcast transforms 我们首先看一下如何把参考系发布到tf。 代码文件：/nodes/turtle_tf_broadcaster.py #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import tf import turtlesim.msg def handle_turtle_pose(msg, turtlename): br = tf.TransformBroadcaster() br.sendTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world") #发布乌龟的平移和翻转 if __name__ == '__main__': rospy.init_node('turtle_tf_broadcaster') turtlename = rospy.get_param('~turtle') #获取海龟的名字（turtle1，turtle2） rospy.Subscriber('/%s/pose' % turtlename, turtlesim.msg.Pose, handle_turtle_pose, turtlename) #订阅 topic "turtleX/pose" rospy.spin() 创建launch文件start_demo.launch： <launch> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" > </node> <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" > </node> </launch> 运行： $ roslaunch learning_tf start_demo.launch 可以看到界面中只有移植乌龟了，打开tf_echo的信息窗口： $ rosrun tf tf_echo /world /turtle1 world参考系的原点在最下角，对于turtle1的转换关系，其实就是turtle1在world参考系中所在的坐标位置以及旋转角度。 三、Writing a tf listener 这一步，我们将看到如何使用tf进行参考系转换。首先写一个tf listener（nodes/turtle_tf_listener.py）： #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import math import tf import turtlesim.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('tf_turtle') listener = tf.TransformListener() #TransformListener创建后就开始接受tf广播信息，最多可以缓存10s rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) turtle_vel.publish(turtlesim.msg.Velocity(linear, angular)) rate.sleep() 在launch文件中添加下面的节点： <launch> ... <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" /> </launch> 然后在运行，就可以看到两只turtle了，也就是我们在最开始见到的那种跟随效果。 四、Adding a frame 在很多应用中，添加一个参考系是很有必要的，比如在一个world参考系下，有很一个激光扫描节点，tf可以帮助我们将激光扫描的信息坐标装换成全局坐标。 1、tf消息结构 tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。 2、建立固定参考系（fixed frame） 我们以turtle1作为父参考系，建立一个新的参考系“carrot1”。代码如下（nodes/fixed_tf_broadcaster.py）： #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import tf if __name__ == '__main__': rospy.init_node('my_tf_broadcaster') br = tf.TransformBroadcaster() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): br.sendTransform((0.0, 2.0, 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "carrot1", "turtle1") #建立一个新的参考系，父参考系为turtle1，并且距离父参考系2米 rate.sleep() 在launch文件中添加节点： <launch> ... <node pkg="learning_tf" type="fixed_tf_broadcaster.py" name="broadcaster_fixed" /> </launch> 运行，还是看到两只乌龟和之前的效果一样。新添加的参考系并没有对其他参考系产生什么影响。打开nodes/turtle_tf_listener.py文件，将turtle1改成carrot1： (trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0)) 重新运行，现在乌龟之间的跟随关系就改变了： 3、建立移动参考系（moving frame） 我们建立的新参考系是一个固定的参考系，在仿真过程中不会改变，如果我们要把carrot1参考系和turtle1参考系之间的关系设置可变的，可以修改代码如下： #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import tf import math if __name__ == '__main__': rospy.init_node('my_tf_broadcaster') br = tf.TransformBroadcaster() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): t = rospy.Time.now().to_sec() * math.pi br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "carrot1", "turtle1") rate.sleep() 这次carrot1的位置现对于turtle1来说是一个三角函数关系了。 代码下载：http://download.csdn.net/detail/hcx25909/5708199
move_base（路径规划） 在上一篇的博客中，我们一起学习了ROS定位于导航的总体框架，这一篇我们主要研究其中最重要的move_base包。 在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分： （1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划； （2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。 一、数据结构 ROS中定义了MoveBaseActionGoal数据结构来存储导航的目标位置数据，其中最重要的就是位置坐标（position）和方向（orientation）。 rosmsg show MoveBaseActionGoal [move_base_msgs/MoveBaseActionGoal]: std_msgs/Header header uint32 seq time stamp string frame_id actionlib_msgs/GoalID goal_id time stamp string id move_base_msgs/MoveBaseGoal goal geometry_msgs/PoseStamped target_pose std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w 二、配置文件 move_base使用前需要配置一些参数：运行成本、机器人半径、到达目标位置的距离，机器人移动的速度，这些参数都在rbx1_nav包的以下几个配置文件中： ? base_local_planner_params.yaml ? costmap_common_params.yaml ? global_costmap_params.yaml ? local_costmap_params.yaml 三、全局路径规划（global planner） 在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。 navfn通过Dijkstra最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。将来在算法上应该还会加入A*算法。 具体见： http://www.ros.org/wiki/navfn?distro=fuerte 注意：indigo版本是： http://wiki.ros.org/navfn?distro=indigo 四、本地实时规划（local planner） 本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。 base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。 其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下： （1） 采样机器人当前的状态（dx,dy,dtheta）； （2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。 （3） 利用一些评价标准为多条路线打分。 （4） 根据打分，选择最优路径。 （5） 重复上面过程。 具体参见： http://www.ros.org/wiki/base_local_planner?distro=groovy 注意：indigo版本是： http://www.ros.org/wiki/base_local_planner?distro=indigo 五、ArbotiX仿真——手动设定目标 在这一步，我们暂时使用空白地图（blank_map.pgm），就在空地上进行无障碍仿真。 首先运行ArbotiX节点，并且加载机器人的URDF文件。 roslaunch rbx1_bringup fake_turtlebot.launch 然后运行move_base和加载空白地图的launch文件（fake_move_base_blank_map.launch）： roslaunch rbx1_nav fake_move_base_blank_map.launch 该文件的具体内容如下： <launch> <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/> <include file="$(find rbx1_nav)/launch/fake_move_base.launch" /> <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" /> </launch> 其中调用了fake_move_base.launch文件，是运行move_base节点并进行参数配置。 然后调用rviz就可以看到机器人了。 rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg 注意：indigo版本为： rosrun rviz rviz -d `rospack find rbx1_nav`/nav_test.rviz 我们先以1m的速度进行一下测试： 让机器人前进一米： rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }' 让机器人后退一米，回到原来的位置： rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }' 在rviz中的轨迹图如下： 在机器人移动过程中，有一条蓝色的线（被黄线挡住了）就是机器人的全局规划的路径；红色的箭头是实施规划的路线，会不断更新，有的时候会呈现很大的弧线，那是因为机器人在转向的过程中尽量希望保持平稳的角度。如果觉得路径规划的精度不够，可以修改配置文件中的pdist_scale参数进行修正。 然后我们可以认为的确定目标位置，点击rviz上方的2D Nav Goal按键，然后左键选择目标位置，机器人就开始自动导航了。 六、ArbotiX仿真——带有障碍物的路径规划 首先我们让机器人走一个正方形的路线。先通过上面的命令，让机器人回到原始位置（0，0，0），然后按reset按键，把所有的箭头清除。接着运行走正方形路径的代码： rosrun rbx1_nav move_base_square.py 在rviz中可以看到： 四个顶角的粉色圆盘就是我们设定的位置，正方形比较规则，可见定位还是比较准确的。然我们先来分析一下走正方形路线的代码： #!/usr/bin/env python import roslib; roslib.load_manifest('rbx1_nav') import rospy import actionlib from actionlib_msgs.msg import * from geometry_msgs.msg import Pose, Point, Quaternion, Twist from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from tf.transformations import quaternion_from_euler from visualization_msgs.msg import Marker from math import radians, pi class MoveBaseSquare(): def __init__(self): rospy.init_node('nav_test', anonymous=False) rospy.on_shutdown(self.shutdown) # How big is the square we want the robot to navigate? # 设定正方形的尺寸，默认是一米 square_size = rospy.get_param("~square_size", 1.0) # meters # Create a list to hold the target quaternions (orientations) # 创建一个列表，保存目标的角度数据 quaternions = list() # First define the corner orientations as Euler angles # 定义四个顶角处机器人的方向角度（Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92) euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions # 将上面的Euler angles转换成Quaternion的格式 for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses # 创建一个列表存储导航点的位置 waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. # 创建四个导航点的位置（角度和坐标位置） waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0])) waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1])) waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2])) waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) # Initialize the visualization markers for RViz # 初始化可视化标记 self.init_markers() # Set a visualization marker at each waypoint # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记) for waypoint in waypoints: p = Point() p = waypoint.position self.markers.points.append(p) # Publisher to manually control the robot (e.g. to stop it) # 发布TWist消息控制机器人 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Subscribe to the move_base action server # 订阅move_base服务器的消息 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available # 等待move_base服务器建立 self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") # Initialize a counter to track waypoints # 初始化一个计数器，记录到达的顶点号 i = 0 # Cycle through the four waypoints # 主循环,环绕通过四个定点 while i < 4 and not rospy.is_shutdown(): # Update the marker display # 发布标记指示四个目标的位置，每个周期发布一起，确保标记可见 self.marker_pub.publish(self.markers) # Intialize the waypoint goal # 初始化goal为MoveBaseGoal类型 goal = MoveBaseGoal() # Use the map frame to define goal poses # 使用map的frame定义goal的frame id goal.target_pose.header.frame_id = 'map' # Set the time stamp to "now" # 设置时间戳 goal.target_pose.header.stamp = rospy.Time.now() # Set the goal pose to the i-th waypoint # 设置目标位置是当前第几个导航点 goal.target_pose.pose = waypoints[i] # Start the robot moving toward the goal # 机器人移动 self.move(goal) i += 1 def move(self, goal): # Send the goal pose to the MoveBaseAction server # 把目标位置发送给MoveBaseAction的服务器 self.move_base.send_goal(goal) # Allow 1 minute to get there # 设定1分钟的时间限制 finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) # If we don't get there in time, abort the goal # 如果一分钟之内没有到达，放弃目标 if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") def init_markers(self): # Set up our waypoint markers # 设置标记的尺寸 marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. # 定义一个标记的发布者 self.marker_pub = rospy.Publisher('waypoint_markers', Marker) # Initialize the marker points list. # 初始化标记点的列表 self.markers = Marker() self.markers.ns = marker_ns self.markers.id = marker_id self.markers.type = Marker.SPHERE_LIST self.markers.action = Marker.ADD self.markers.lifetime = rospy.Duration(marker_lifetime) self.markers.scale.x = marker_scale self.markers.scale.y = marker_scale self.markers.color.r = marker_color['r'] self.markers.color.g = marker_color['g'] self.markers.color.b = marker_color['b'] self.markers.color.a = marker_color['a'] self.markers.header.frame_id = 'map' self.markers.header.stamp = rospy.Time.now() self.markers.points = list() def shutdown(self): rospy.loginfo("Stopping the robot...") # Cancel any active goals self.move_base.cancel_goal() rospy.sleep(2) # Stop the robot self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) if __name__ == '__main__': try: MoveBaseSquare() except rospy.ROSInterruptException: rospy.loginfo("Navigation test finished.") 但是，在实际情况中，往往需要让机器人自动躲避障碍物。move_base包的一个强大的功能就是可以在全局规划的过程中自动躲避障碍物，而不影响全局路径。障碍物可以是静态的（比如墙、桌子等），也可以是动态的（比如人走过）。 现在我们尝试在之前的正方形路径中加入障碍物。把之前运行除了roscore，其他终端Ctrl-C掉，然后运行： roslaunch rbx1_nav fake_move_base_obstacle.launch 注意：indigo版本为： $ roslaunch rbx1_nav fake_move_base_map_with_obstacles.launch 在运行RVIZ： $ rosrun rviz rviz -d `rospack find rbx1_nav`/nav_obstacles.rviz 然后就会看到在rviz中出现了障碍物。然后在运行之前走正方形路线的代码： rosrun rbx1_nav move_base_square.py 这回我们可以看到，在全局路径规划的时候，机器人已经将障碍物绕过去了，下过如下图： 在上图中，黑色的线是障碍物，周围浅色的椭圆形是根据配置文件中的inflation_radius参数计算出来的安全缓冲区。全局规划的路径基本已经是最短路径了。在仿真的过程中也可以动态重配置那四个配置文件，修改仿真参数。