ROS slam gmapping不能画地图



旧版本slam可能有问题，更新slam包就好了。

https://github.com/ros-perception/slam_gmapping

放到catkin_ws/src目录下

执行catkin_make

到catkin_ws/devel/lib/gmapping/目录下复制编译后的文件到/opt/ros/indigo/lib/gmapping目录下替换

2.

明：本文为博主原创文章，未经博主允许不得转载。

启动出错：roslaunch turtlebot_gazebo amcl_demo.launch map_file:=/tmp/my_map.yaml

core service [/rosout] found

process[map_server-1]: started with pid [4654]

process[amcl-2]: started with pid [4675]

*** Error in `/opt/ros/indigo/lib/amcl/amcl': malloc(): smallbin double linked list corrupted: 0x09317188 ***

process[navigation_velocity_smoother-3]: started with pid [4783]

process[kobuki_safety_controller-4]: started with pid [4857]

process[move_base-5]: started with pid [4931]

[ WARN] [1452850112.087962534, 201.010000000]: Detected jump back in time. Clearing TF buffer.

[ WARN] [1452850112.206104792, 201.130000000]: Detected jump back in time. Clearing TF buffer.

[amcl-2] process has died [pid 4675, exit code -6, cmd /opt/ros/indigo/lib/amcl/amcl scan:=scan __name:=amcl __log:=/home/exbot/.ros/log/ce981b1c-bb69-11e5-ade6-4487fc759faa/amcl-2.log].

log file: /home/exbot/.ros/log/ce981b1c-bb69-11e5-ade6-4487fc759faa/amcl-2*.log

[move_base-5] process has died [pid 4931, exit code -11, cmd /opt/ros/indigo/lib/move_base/move_base cmd_vel:=navigation_velocity_smoother/raw_cmd_vel odom:=odom scan:=scan __name:=move_base __log:=/home/exbot/.ros/log/ce981b1c-bb69-11e5-ade6-4487fc759faa/move_base-5.log].

log file: /home/exbot/.ros/log/ce981b1c-bb69-11e5-ade6-4487fc759faa/move_base-5*.log

解决办法：amcl包的版本问题  

重新下载一个amcl包 编译后替换以前的amcl

注意下载时选择相匹配的版本

https://github.com/ros-planning/navigation

放到catkin_ws/src目录下

执行catkin_make

到catkin_ws/devel/lib/amcl/目录下复制编译后的文件到/opt/ros/indigo/lib/amcl目录下替换

     3.Kobuki（turtlebot底盘）通信方式

当Kobuki_nodelet被运行时，自动跳转到onInit()函数，函数中建立KobukiRos类的新对象，并调用其init() 函数，在KobukiRos类的init()函数中，定义了若干个用于接收ROS话题的subscriber，若干个用于发布底盘传感器数据的publisher。并且启动了Kobuki这个驱动类的初始化函数。

    最后启动KobukiRos对象的update()函数，用于检测底盘的各种状态，比如底盘的连接状态，电池状态，轮子是否接触地面等等。

Turtlebot的运行机制总结如下：

    启动minimal.launch之后，启动Kobuki_nodelet节点，新建两个类对象，一是用于底盘和ROS通信类KobukiRos（桥梁），一个是底盘的驱动类Kobuki（心脏）。

通信类中定义了用于接收ROS命令话题的接收者，接收如/cmd_vel等话题。接收者接收到话题消息后，通过回调函数，将ROS通用格式的数据转换为底盘可用格式的数据，并调用驱动类的函数接口，通过串口给底盘单片机发送数据，实现对底盘的操控。

通信类中还定义了用于发布底盘状态话题的发布者，如发送/sensors/imu_data的话题。通信类通过驱动类的函数接口获得原始传感器数据，将其转换为ROS通用数据格式，由发布者发送给ROS上层，来实现反馈功能。

最后，Kobuki_nodelet启动了一套完整的监测机制，用于监测底盘的各种异常状态。

参考源码：

kobuki_nodelet.cpp:（实现Kobuki_nodelet类）

https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/nodelet/kobuki_nodelet.cpp

kobuki_ros.hpp:

https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/include/kobuki_node/kobuki_ros.hpp

kobuki_ros.cpp：（实现KobukiRos类，相当于桥梁）

https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/kobuki_ros.cpp

subscriber_callbacks.cpp:（实现回调函数，ROS数据格式和底盘数据格式的转换）

https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/subscriber_callbacks.cpp

kobuki.hpp:

https://github.com/yujinrobot/kobuki_core/blob/indigo/kobuki_driver/include/kobuki_driver/kobuki.hpp

kobuki.cpp:（实现Kobuki类，驱动类，相当于心脏）

https://github.com/yujinrobot/kobuki_core/blob/indigo/kobuki_driver/src/driver/kobuki.cpp

详细跳转机制：（以/cmd_vel为例）

    在Kobuki_nodelet.cpp中，首先新类KobukiRos对象。然后调用KobukiRos的init()初始化函数，在这个函数中，声明了许多发布者和订阅者，下面以/cmd_vel话题为例，继续深入。

    velocity_command_subscriber= nh.subscribe(std::string("commands/velocity"), 10,&KobukiRos::subscribeVelocityCommand, this);

    当有/cmd_vel话题发布时，这个发布者的回调函数自动被调用，即进入KobukiRos::subscribeVelocityCommand（）中。

    在回调函数中，其将数据发送给驱动类Kobuki的函数中：

    kobuki.setBaseControl(msg->linear.x, msg->angular.z);

    kobuki.setBaseControl这个函数通过调用diff_driver类（差速轮驱动）的setVelocityCommands()函数，将/cmd_vel话题的Twist类型数据转换为左右轮速，并保存在vector容器中。

    注意，diff_driver::setVelocityCommands()并没有直接将左右轮速通过串口发送给底盘，而是保存在vector<double>类型的私有成员point_velocity之中。

    为什么没有直接通过串口传输呢，那传输步骤在哪呢？我们接着来看，以上的步骤其实仅仅是一个线程，我们再来看看另外的线程。

    继续回到KobukiRos的init()初始化函数，除了声明了发布者和订阅者用于接收发布数据，同时也调用了驱动类Kobuki的init（）初始化函数。

    在驱动类Kobuki的init()初始化函数中，初始了串口，并设置了一些必要信息，最后进入了Kobuki::spin()循环函数。

    在spin()函数中，首先判断了是否打开了串口，如果没打开就打开并设置串口。确认完毕之后，先做读操作，然后做写操作。在写操作中，调用了sendBaseControlCommand()，在这个函数中，先调用了上面讲过的diff_driver：：velocityCommands()函数，将转换好的数据设置在Kobuki类的vector<double>类型的私有成员velocity_commands中。最后调用sendCommand()函数，终于在里面看到了串口写操作的相关代码！！！

4.运动规划 (Motion Planning): MoveIt! 与 OMPL

最近有不少人询问有关MoveIt!与OMPL相关的话题，但是大部分问题都集中于XXX功能怎么实现，XXX错误怎么解决。表面上看，解决这些问题的方法就是提供正确的代码，正确的编译方法，正确的运行步骤。 

然而，这种解决方法只能解决这个特定的问题，而且解决之后我们也无法学到一些实际的东西。要想彻底明白，需要从源头入手，也就是说，不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点？“，而是要问”MoveIt!  为什么能把机械手从空间一个点移到另一个点？“。 这一点明白之后，遇到类似的问题，才能从容应对。同理，这不仅适用于MoveIt，也同样适用于其他任何ROS功能。 

所以，下文中我们会见到一些具体的例子，但整体上，更倾向于宏观的概念和一些基础的方法，希望对大家能有所帮助。这里的帮助指的是增强对运动规划和Moveit, OMPL的整体理解，而非局限于完成某一个功能，编译运行某一个文件。 


我尝试用最简单，最通俗的表达方式来解释这些问题，其中不免会有一些学术上的错误用词和解释，请专业的朋友们见谅，也欢迎指出错误。 



一. 基础概念 


首先，我们要了解一些基础的概念，了解各个名词的意义和区别。 


1.1. 运动规划 (Motion Planning) 
我们这里讲的运动规划，有别于轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。 


既然存在无数的解，那么问题来了。很明显，存在两种不同的方向，一种是找到最好的那个解，另一种是快速的找到一个有效的解。前者，大部分算法使用最优规划 (Optimization-based Planning)，后者使用采样规划 (Sampling-based Planning)。具体的区别和算法，不在这里赘述。 


1.2. 开源运动规划库 (OMPL). 
接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但总体来说OMPL还是一个采样规划算法库。而采样规划算法中，最出名的莫过于  Rapidly-exploring Random Trees (RRT)  和 Probabilistic Roadmap (PRM)了,  当然，这两个是比较老的，还有很多其他新算法。 

• OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。
• 为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

1.3. 逆运动学 (Inverse Kinematics)

• 什么是逆运动学(IK)？简单说，就是把终端位姿变成关节角度，q=IK(p)。p是终端位姿(xyz)，q是关节角度。
• 为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,...qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。

1.4. MoveIt! 
问：我不想看也看不懂OMPL和各种算法，但是我想让机械臂动起来，怎么办？ 
答：那这正是MoveIt!的设计初衷。Move It！让它动起来！ 
OMPL是运动规划的“规划”部分，而MoveIt!是OMPL的ROS接口。当然这不完全准确，OMPL有单独的ROS接口，但依旧很繁杂，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。 

• MoveIt!能做什么？一句话，MoveIt!就是一个模块化的接口，让你在最短时间内，不用自己写太多代码，就能配置出一个ROS Package来为你的机械臂做运动规划。

二. 创建MoveIt! Package 

2.1 准备URDF package 
首先我们要准备一个机械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手头没有现成的URDF，可以从此处下载一个库卡LWR简化模型URDF，这是一个固定底座7自由度的机械臂。 
从该连接处依次进入examples/sovlers/ik_solver_demo/resources，下载里面的lwr_simplified.urdf。 

复制代码

1

2

3

4

cd path_to_catkin_ws/src

catkin_create_pkg lwr_description

cd lwr_description

mkdir urdf

将下载好的lwr_simplified.urdf放入urdf文件夹中，这样一个urdf package便创建好了。 


2.2 MoveIt!配置助手 (MoveIt! Setup Assistant) 
2.2.1 打开MoveIt! Setup Assistant 


复制代码

1

roslaunch moveit_setup_assistant setup_assistant.launch

 

MoveIt! Setup Assistant 是一个图形界面，可以让我们不用写代码看代码，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包，选择刚刚下载的urdf，然后点击Load Files 载入文件。 

2.2.2 创建碰撞免检矩阵(ACM) 

点击Setup Assisant的左边第二项'Self-Collisions'，在这里我们将创建碰撞免检矩阵(Avoid Collision Matrix, ACM)。再次强调，怎么创建很简单，点击一下'Regenerate Default Collision Matrix'就可以了，问题是，为什么？ACM是做什么的？ 
我们知道，碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。 


2.2.3 创建虚拟关节 (Virtual Joints) 
在Setup Assistant 第三项Virtual Joints里面，我们要创建所谓的虚拟关节。这个虚拟关节，可以理解为一个连接机器人和世界的关节。

一般来说，Virtual Joint Name我们命名为‘world_joint’，而'Child Link'指的是我们要把‘世界’和机器人的那个部位连接，那么很显然我们选择基座'base'。‘Parent Frame Name’，是世界坐标的名字，在ROS中一般叫'world_frame'。关节类型 Joint Type, 很显然这里我们选择固定Fixed. 代表机器人相对于世界是固定的。而另外两种, Planar指的是平面移动底座(xy平面+角度)，用于移动机器人比如PR2；

还有一种Floating, 指的是浮动基座(xyz position+orientation)，比如类人机器人。

2.2.4 创建规划群 (Planning Groups)

创建Planning Group是MoveIt的核心之一。首先，点击Add Group, 我们会看到一个界面，如下图，

 

那么这些都是什么呢？

• Group Name: 不用多说，名字。。。我们就叫Arm。
• Kinematic Solver: 运动学求解工具，这个就是负责求解正向运动学(Forward Kinematics)和逆运动学(IK, 见1.3节)的。 一般我们选用KDL， The Kinematics and Dynamics Library。这是一个运动学与动力学的库，可以很好的解决6自由度以上的单链机械结构的正逆运动学问题。当然你也可以用其他IK Solver, 比如SRV或者<span \"="" style="box-sizing: border-box;">IK_FAST，甚至你可以自己开发新的Solver然后插入进来，如果有空，我以后会发帖讲解如何创建新的运动学求解库并插入到MoveIt。
• Kin. Search Resolution: 关节空间的采样密度
• Kin. Search TImeout: 求解时间
• Kin. Solver Attempts: 求解失败尝试次数，一般来说这三项使用默认值就可以。你也可以根据具体需要做出适当调整。

接下来，我们要正式创建这个组群，有很多不同的方法，Add Joints, Add Links, Add Kin. Chain, Add Subgroups。我们这里选择'Add Kin. Chain'，这样我们可以清楚的看到整个机器人的机械机构，

在正中方我们可以看到这个机械臂的结构，一个link接着一个link。下方我们可以看到有'Base Link'和'Tip Link'，我们选择'lwr_arm_0_link'作为Base，选择'lwr_arm_7_link'作为Tip. 然后点击Save，这样一个规划组群就创建好了。同样的，我们可以再创建一个手的组群(Hand)，这一次我们用Add Links，然后选择'lwr_arm_7_link'。

 

2.2.5 创建机器人预设位姿 (Robot Poses)

在Setup Assistant 第五项, ‘Robot Poses’，我们创建预设的机器人位姿。点击‘Add Pose’，我们为机械臂创建一个向上直立的位姿UpRight，选择Planning Group为Arm。可以看到很多滚动条，全设为0就是垂直向上的位姿。然后点击保存。当然，你可以根据需要设置其他不同位姿。

2.2.6 配置终端控制器(End Effectors)

终端控制器，就是机械臂的手，以后用来在工作环境中直接控制的部位。我们添加一个叫做HandEff的终端控制器，End Effector Group选择之前创建好的Hand，Parent_Link选择机械臂的最后一个肢体lwr_arm_7_link。Parent Group选择Arm。

2.2.7 配置被动关节(Passive Joints) 
所谓被动关节，就是指现实中不配置电机的关节，也就是不会出现在机器人的Joint State Msg里，以避免MoveIt与JointState出现匹配错误。这里我们的LWR机械臂并没有此类被动关节，所以可以直接跳过。 

2.2.8 生成配置文件(Configuration Files) 
最后一步，在Configuration Package Save Path里面选择一个保存地址，一般我们把他放在path_to_catkin_ws/src/lwr_moveit_config然后点击Generate Package，这样一个完整的MoveIt Configuration Package就创建好了！先不要急着运行，我们先来看看都生成了哪些东西，还有一些重要的配置参数都是在哪定义的。

三. MoveIt 配置包详解 
打开刚刚创建好的lwr_moveit_config文件夹，我们发现有config和launch两个文件夹。3.1 MoveIt! 配置文件先看config，里面有

• fake_controllers.yaml：这是虚拟控制器配置文件，方便我们在没有实体机器人，甚至没有任何模拟器（如gazebo）开启的情况下也能运行MoveIt。
• joint_limits.yaml：这里记录了机器人各个关节的位置速度加速度的极限，这些都会被用于以后的规划中。
• kinematics.yaml：这里就是上一章2.2.4里面设置的东西，用于初始化运动学求解库
• lwr.srdf：这个是一个重要的MoveIt配置文件，我们将在下一节详解。
• ompl_planning.yaml：这里是配置OMPL各种算法的各种参数。

3.2 SRDF文件 
SRDF是moveit的配置文件，配合URDF使用。打开lwr.srdf，

我们可以看到这是一个xml格式的配置文件，根是robot，并有一个属性值name='lwr'。下面各个项目应该很明显，就是我们刚刚在Setup Assistant里面所设置的东西，包含了组群，位姿，终端控制器，虚拟关节，以及碰撞免测矩阵ACM的定义。理论上，只要有了srdf和urdf，我们就可以完全定义一个机器人moveit信息。

3.3 Launch文件 

下面，我们看看launch文件夹，一打开发现有很多文件，瞬间不想看了。。不要急，我们来看看几个重要的文件。 


3.3.1 demo.launch 
demo是运行的总结点，打开我们可以看到他include了其他的launch文件。其中第14行说，如果有需要，发布静态的tf。比如说，你的机器人基座不在世界坐标的原点，你可以发布一个静态tf来描述机器人在世界坐标中的位置。第17-21行，就是我们发布虚拟机器人状态的地方了，当然，如果你有实体机器人或者有gazebo之类的模拟器，你需要去掉这一部分，有其他相应的节点来发布机器人状态。26-32行运行了另一个moveit重要的节点，move group。 


3.3.2 move_group.launch 
顾名思义，move group的功能是让一个规划组群动起来。怎么动，那就要做运动规划了，在move_group.launch第24-26行定义了运动规划库的使用，我们可以看到，默认的是使用ompl运动规划库。同样的，如果以后有时间，我会发帖详解如何创建新的运动规划库插件并让moveit使用其他的运动规划算法。其他的都是设置一些基本参数，暂时可以略过。 


3.3.3 planning_context.launch 
这里我们可以看到，定义了所使用的urdf和srdf文件，以及运动学求解库。不建议手动更改这些，但是如果你需要使用不同的urdf，srdf，可以在这里更改。 


3.3.4 setup_assistant.launch 
如果你需要更改一些配置，那么可以直接运行 

复制代码

1

roslaunch lwr_moveit_config setup_assistant.launch

这样就可以基于当前设置做更改，而不是重新设置。 

四. 运行MoveIt! 

4.1 Launch Demo 
现在我们可以来尝试运行moveit了！ 

复制代码

1

roslaunch lwr_moveit_config demo.launch

等待几秒，当看到 All is well! Everyone is happy! You can start planning now! 的时候，就代表启动成功了。我们可以看到一个Rivz窗口，左下角有一个运动规划MotionPlanning模块。

 

第一个进入视野的就是Planning Library, OMPL。没错，这里告诉你当前用的是OMPL运动规划算法。在中间的下来菜单里面有很多的具体算法，之后你可以尝试不同的算法，看看他们的区别。

4.2 选择目标位姿

如果如上文第二章中设置，你会在rviz主窗口中看到一个互动标记位于机械臂终端位置。移动这个标记到另外一个地方，你可以看到一个橙色的目标位姿(每一次移动标记，就运行了一次逆运动学IK求解过程)。

同样的，你也可以在MotionPlanning模块下的Planning子模块写的Query子模块里面设置随机的或者预设的目标位置。

4.3 运动规划终于，到了运动规划的时候了。。在Planning子模块中单击Plan，一个运动轨迹就会出现与Rviz窗口中并循环播放。你可以在Display->MotionPlanning->Planned Path里面设置各种显示参数。

同时，在terminal里面我们可以看到一些输出

复制代码

1

2

3

4

5

6

7

8

9

10

11

12

13

14

[ INFO] [1453481861.884163555]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1453481861.884336258]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1453481861.884489778]: LBKPIECE1: Starting planning with 1 states already in datastructure

[ INFO] [1453481861.884523826]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1453481861.884547702]: LBKPIECE1: Starting planning with 1 states already in datastructure

[ INFO] [1453481861.884564358]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1453481861.884587404]: LBKPIECE1: Starting planning with 1 states already in datastructure

[ INFO] [1453481861.884604829]: LBKPIECE1: Attempting to use default projection.

[ INFO] [1453481861.884626253]: LBKPIECE1: Starting planning with 1 states already in datastructure

[ INFO] [1453481861.905034917]: LBKPIECE1: Created 99 (46 start + 53 goal) states in 88 cells (45 start (45 on boundary) + 43 goal (43 on boundary))

[ INFO] [1453481861.905633020]: LBKPIECE1: Created 87 (33 start + 54 goal) states in 76 cells (31 start (31 on boundary) + 45 goal (45 on boundary))

[ INFO] [1453481861.913846457]: LBKPIECE1: Created 126 (76 start + 50 goal) states in 115 cells (75 start (75 on boundary) + 40 goal (40 on boundary))

[ INFO] [1453481861.914639489]: LBKPIECE1: Created 220 (72 start + 148 goal) states in 201 cells (70 start (70 on boundary) + 131 goal (131 on boundary))

[ INFO] [1453481861.948016518]: ParallelPlan::solve(): Solution found by one or more threads in 0.063719 seconds

我们可以看到，本次运动规划使用了LBKPIECE算法，并且使用了4线程并行规划，规划时间为0.063719秒。你也可以在OMPL算法里选择其他规划算法比如RRT，RRT-Connect，PRM，EST等等等等。。看看他们之间的轨迹区别和速度区别。

4.4 避障运动规划 Collision Free Motion Planning

上面这个简单的运动规划，环境中并没有障碍物。下面我们来为环境中增加几个物体。创建一个demo.scene文件，将以下代码复制进去（你可以自己设计不同的scene）

复制代码

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

(noname)+++++

* obs1.dae

1

mesh

24 12

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 0.0315993

0 1 2

1 3 2

4 5 6

4 7 5

8 9 10

11 8 10

12 13 14

12 14 15

16 17 18

19 16 18

20 21 22

20 22 23

0.7 0 0.8

0.706825 0 0 0.707388

0.5 0 0 1

* obs2.dae

1

mesh

24 12

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 0.0315993

0 1 2

1 3 2

4 5 6

4 7 5

8 9 10

11 8 10

12 13 14

12 14 15

16 17 18

19 16 18

20 21 22

20 22 23

0.6 0.5 0.8

0.706825 0 0 0.707388

0.5 0 0 1

* obs3.dae

1

mesh

24 12

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 0.0315993

0 1 2

1 3 2

4 5 6

4 7 5

8 9 10

11 8 10

12 13 14

12 14 15

16 17 18

19 16 18

20 21 22

20 22 23

0.3 0 0.8

0.999784 0 0 0.0207948

0.5 0 0 1

* table.dae

1

mesh

24 12

-0.420619 0.0420619 -0.841237

-0.420619 -0.0420619 -0.841237

-0.420619 0.0420619 0.841237

-0.420619 -0.0420619 0.841237

0.420619 0.0420619 0.841237

0.420619 -0.0420619 -0.841237

0.420619 0.0420619 -0.841237

0.420619 -0.0420619 0.841237

-0.420619 0.0420619 0.841237

-0.420619 -0.0420619 0.841237

0.420619 -0.0420619 0.841237

0.420619 0.0420619 0.841237

0.420619 -0.0420619 -0.841237

-0.420619 -0.0420619 -0.841237

-0.420619 0.0420619 -0.841237

0.420619 0.0420619 -0.841237

-0.420619 -0.0420619 0.841237

-0.420619 -0.0420619 -0.841237

0.420619 -0.0420619 -0.841237

0.420619 -0.0420619 0.841237

0.420619 0.0420619 -0.841237

-0.420619 0.0420619 -0.841237

-0.420619 0.0420619 0.841237

0.420619 0.0420619 0.841237

0 1 2

1 3 2

4 5 6

4 7 5

8 9 10

8 10 11

12 13 14

15 12 14

16 17 18

19 16 18

20 21 22

20 22 23

0.7 0 0.63

0.706825 0 0 0.707388

0 0.5 0.5 1

* target

1

mesh

24 12

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

-0.0315993 -0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 -0.126397 0.0315993

-0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 -0.0315993

0.0315993 -0.126397 0.0315993

0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 -0.0315993

-0.0315993 0.126397 0.0315993

0.0315993 0.126397 0.0315993

0 1 2

1 3 2

4 5 6

4 7 5

8 9 10

11 8 10

12 13 14

12 14 15

16 17 18

19 16 18

20 21 22

20 22 23

0.6 0.2 0.8

0.706825 0 0 0.707388

0 1 0 1

.

然后在SceneObjects模块中点击ImportFromText，选择刚刚创建的demo.scene文件，一个简单的桌面环境就被导入进了rviz。你可以通过选择各个物体，来调整他们的位置。

回到Context子模块，点击Publish Current Scene，将当前的环境发布出去。

然后再次点击Plan，你会看到一条不同的轨迹，这一轨迹应该绕过所有障碍物并且达到目标位姿。

因为OMPL是采样算法，由于其随机采样的特性，每次的路径是不同的，如下图。而且有可能失败。

接下来可以尝试不同的OMPL算法，不同的目标位姿和不同的环境。来看看MoveIt的鲁棒性如何。 


那么，MoveIt!和OMPL的运动规划就差不多讲完了，当然这是很浅显的，与实用性的东西都还有距离。以上都是纯手打，现做的例子，希望有所帮助。接下来该讲什么，你们可以在下面留言，是更深入的讲MoveIt!，还是讲OMPL运动规划算法，还是讲如何模拟具体实例。