ROS探索总结（二十五）~（三十）(转）

（二十五）——MoveIt基础

MoveIt！由ROS中一系列移动操作的功能包组成，包含运动规划，操作控制，3D感知，运动学，碰撞检测等等，而且提供友好的GUI。官方网站：，上边有MoveIt！的教程和API说明。

一、架构

下图是MoveIt的总体框架:

move_group是MoveIt的核心部分，可以综合机器人的各独立组件，为用户提供一系列需要的动作指令和服务。从架构图中我们可以看到，move_group类似于一个积分器，本身并不具备丰富的功能，主要做各功能包、插件的集成。它通过消息或服务的形式接收机器人上传的点云信息、joints的状态消息，还有机器人的tf tree，另外还需要ROS的参数服务器提供机器人的运动学参数，这些参数会在使用setup assistant的过程中根据机器人的URDF模型文件，创建生成（SRDF和配置文件）。

官网的说明：

二、安装

MoveIt的安装很简单，使用下边的命令即可：

sudo apt-get install ros-indigo-moveit-full

下边会使用《master ros for robotics programming》里的案例进行学习，在工作空间里git下来源码：

git clone https://github.com/qboticslabs/mastering_ros.git

三、setup配置

使用MoveIt!的第一步是要使用 Setup Assistant工具完成一些配置工作。Setup Assistant会根据用户导入的机器人的urdf模型，生成SRDF( Semantic Robot Description Format)文件，从而生成一个MoveIt!的功能包，来完成机器人的互动、可视化和仿真。

首先，来运行setup assistant：

rosrun moveit_setup_assistant moveit_setup_assistant

在出现的界面左侧，可以看到我们接下来需要配置的步骤。

3.1 加载URDF

这里有两个选择，一个是新建一个配置功能包，一个是使用已有的配置功能包，我们选择新建，在源码中找到urdf文件。如果已有配置功能包，那么新建后会覆盖原有的文件。

从右边的窗口我们看到导入的机器人模型。

3.2 Self-Collision

然后点击左侧的第二项配置步骤，配置自碰撞矩阵，MoveIt！可以让我们设置一定数量的随机采样点，根据这些点生成配装参数，可想而知，过多的点会造成运算速度慢，过少的点会导致参数不完善，默认的采样点数量是10000个，官方称经过实践，要获得不错的效果，10000是最低要求，我们就按照这个默认值生成碰撞矩阵。

3.3 Virtual Joints

虚拟关节主要是用来描述机器人在world坐标系下的位置，如果机器人是移动的，虚拟关节可以与移动基座关联，不过一般的机械臂都是固定不动的，所以也可以不需要虚拟关节。

3.4 Planning Groups

这一步可以将机器人的多个组成部分（links，joints）集成到一个组当中，运动规划会针对一组links或joints完成运动规划，在配置个过程中还可以选择运动学解析器（ kinematic solver）。这里创建两个组，一个是机械臂部分，一个是夹具部分。

3.5 Robot Poses

这里可以设置一些固定的位置，比如说机器人的零点位置、初始位置等等，当然这些位置是用户根据场景自定义的，不一定要和机器人本身的零点位置、初始位置相同。这样做的好处就是我们使用MoveIt！的API编程时，可以直接调用这些位置。

3.6 End-Effectors

机械臂在一些实用场景下，会安装夹具等终端结构，可以在这一步中添加。

3.7 Passive Joints

机器人上的某些关节，可能在规划、控制过程中使用不到，可以先声明出来，这里没有就不用管了。

3.8 Configuration Files

最后一步，生成配置文件。一般会取名 robot_name_moveit_config。这里，会提示覆盖已有的配置文件。

OK，现在已经完成配置了，可以退出 Setup Assistant，运行demo看看：

这个界面是在rviz的基础上加入了MoveIt！插件，通过左下角的插件，我们可以选择机械臂的目标位置，进行运动规划。

拖动机械臂的前端，可以改变机械臂的姿态，在planning页面，点击“Plan and Execute”，MoveIt！开始规划路径，并且可以看到机械臂开始向目标位置移动了！

四、运动规划

看过直观效果之后，我们来分析一下运作的机理。

假设我们已知机器人的初始姿态和目标姿态，以及机器人和环境的模型参数，那么我们就可以通过一定的算法，在躲避环境障碍物和放置自身碰撞的同时，找到一条到达目标姿态的较优路径，这种算法就称为机器人的运动规划。机器人和环境的模型静态参数由URDF文件提供，在默写场景下还需要加入3D摄像头、激光雷达来动态检测环境变化，避免与动态障碍物发生碰撞。

4.1 motion_planner

在MoveIt中，运动规划算法由运动规划器完成，当然，运动规划算法有很多，每一个运动规划器都是MoveIt的一个插件，可以根据需求选用不同的规划算法。，move_group 默认使用的是OMPL算法（）。

运动规划的流程如上图所示，首先我们需要发送一个运动规划的请求（比如说一个新的终端位置）给运动规划器，当然，运动规划也不能随意计算，我们可以根据实际情况，设置一些约束条件：

l 位置约束：约束link的位置

l 方向约束：约束link的运动方向

l 可见性约束：约束link上的某点在某区域的可见性（通过视觉传感器）

l joint约束：约束joint的运动范围

l 用户定义约束：用户通过回调函数自定义一些需要的约束条件。

根据这些约束条件和用户的规划请求，运动规划器通过算法计算求出一条合适的运动轨迹，并回复给机器人控制器。

上图的运功规划器两侧，还分别有一个planning request adapters，这个东东是干什么的呢？从名称上我们大概可以猜出来，planning request adapters作为一个适配器接口，主要功能是预处理运功规划请求和响应的数据，使之满足规划和使用的需求。adapters带有一个s，可见适配器的种类还不只一种，以下是MoveIt！默认使用的一些适配器：

l FixStartStateBounds：如果一个joint的状态稍微超出了joint的极限，这个adapter可以修复joint的初始极限。

l FixWorkspaceBounds：这个adapter可以设置一个10m*10m*10m的规划运动空间。

l FixStartStateCollision：如果已有的joint配置文件会导致碰撞，这个adapter可以采样新的碰撞配置文件，并且根据一个 jiggle_factor因子修改已有的配置文件。

l FixStartStatePathConstraints：如果机器人的初始姿态不满足路径约束，这个adapter可以找到附近满足约束的姿态作为机器人的初始姿态。

l AddTimeParameterization：运动规划器规划得出的轨迹只是一条空间路径，这个adapter可以为这条空间轨迹进行速度、加速度约束，我们可以通过rostopic echo查看规划的路径数据，这个adapter其实就是把空间路径按照距离等分，然后在每个点加入速度、加速度、时间等参数。

4.2 Planning Scene

planning scene可以为机器人创建一个具体的工作环境，加入一些障碍物。

这一功能主要由move group节点中的planning scene monitor来实现，主要监听：

l State Information： joint_states topic

l Sensor Information： the world geometry monitor，视觉传感器采集周围环境数据

l World geometry information： planning_scene topic

4.3 Kinematics

运动学算法是机械臂各种算法中的核心，尤其是反向运动学算法（ inverse kinematics， IK）。MoveIt！使用插件的形式可以让用户灵活的选择需要使用的反向运动学算法，也可以选择自己的算法。

MoveIt！中默认的IK算法是 numerical jacobian-based算法。关于算法，可以参考：

4.4 Collision Checking

MoveIt使用 CollisionWorld 对象进行碰撞检测，采用FCL（ Flexible Collision Library，）功能包。碰撞检测是运动规划中最耗时的运算，往往会占用90%左右的时间，为了减少计算量，可以通过设置ACM( Allowed Collision Matrix )来进行优化，如果两个bodys之间的ACM设置为1，则意味着这两个bodys永远不会发生碰撞，不需要碰撞检测。

（二十六）——MoveIt编程

在之前的基础学习中，我们已经对moveit有了一个基本的认识，在实际的应用中，GUI提供的功能毕竟有限，很多实现还是需要我们在代码中完成，moveit的move_group也提供了丰富的C++ API，不仅可以帮助我们使用代码完成GUI可以实现的功能，还可以加入更多丰富的功能。我们继续使用《Mastering ROS for robotics Programming》中的源码作为学习对象。

一、创建功能包

首先，我们先创建一个新的功能包，来放置我们的代码：

$ catkin_create_pkg seven_dof_arm_test catkin cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs

也可以直接使用《Mastering ROS for robotics Programming》中的seven_dof_arm_test功能包。

二、随机轨迹

通过rviz的planning插件的功能，我们可以为机器人产生一个随机的目标位置，让机器人完成运动规划并移动到目标点。使用代码同样可以实现相同的功能，我们首先从这个较为简单的例程入手，对Moveit C++ API有一个初步的认识。

二话不说，先上代码（源码文件 test_random.cpp可以在源码包中找到）：

//首先要包含API的头文件

#include <moveit/move_group_interface/move_group.h>

int main(int argc, char **argv)

{

ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);

// 创建一个异步的自旋线程（spinning thread）

ros::AsyncSpinner spinner(1);

spinner.start();

// 连接move_group节点中的机械臂实例组，这里的组名arm是我们之前在setup assistant中设置的

move_group_interface::MoveGroup group("arm");

// 随机产生一个目标位置

group.setRandomTarget();

// 开始运动规划，并且让机械臂移动到目标位置

group.move();

ros::waitForShutdown();

}

已经在代码中加入了重点代码的解释，move_group_interface::MoveGroup用来声明一个机械臂的示例，后边都是针对该实例进行控制。

除了moveit，可能很多人对ROS单节点中的多线程API接触的比较少。一般我们使用的自旋API都是spin()或者spinonce()，但是有些情况下会有问题，比如说我们有两个回调函数，第一个回调函数会延时5秒，那么当我们开始spin()时，回调函数会顺序执行，第二个回调函数会因为第一个回调函数的延时，在5秒之后才开始执行，这个当然是我们无法接受的，所如果采用多线程的spin()，就不会存在这个问题了。关于ROS多线程的问题，可以参考wiki等资料：

http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

http://www.cnblogs.com/feixiao5566/p/5288206.html

然后修改CMakeLists文件，编译代码，执行下边的命令：

$ roslaunch seven_dof_arm_config demo.launch

$ rosrun seven_dof_arm_test test_random_node

稍等一下，我们就可以在rviz中看到机械臂的动作了。

三、自定义目标位置并完成规划

接下来我们继续学习如何使用API自定义一个目标位置并让机器人运动过去。源码是 test_custom.cpp，这里我删掉了部分冗余的代码，进行了部分修改：

// 包含miveit的API头文件

#include <moveit/move_group_interface/move_group.h>

int main(int argc, char **argv)

{

ros::init(argc, argv, "move_group_interface_tutorial");

ros::NodeHandle node_handle;

ros::AsyncSpinner spinner(1);

spinner.start();

moveit::planning_interface::MoveGroup group("arm");

// 设置机器人终端的目标位置

geometry_msgs::Pose target_pose1;

target_pose1.orientation.w = 0.726282;

target_pose1.orientation.x= 4.04423e-07;

target_pose1.orientation.y = -0.687396;

target_pose1.orientation.z = 4.81813e-07;

target_pose1.position.x = 0.0261186;

target_pose1.position.y = 4.50972e-07;

target_pose1.position.z = 0.573659;

group.setPoseTarget(target_pose1);

// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动

moveit::planning_interface::MoveGroup::Plan my_plan;

bool success = group.plan(my_plan);

ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");

//让机械臂按照规划的轨迹开始运动。

//if(success)

// group.execute(my_plan);

ros::shutdown();

return 0;

}

对比生成随机目标的源码，基本上只有只是加入了设置终端目标位置的部分代码，此外这里规划路径使用的是plan()，这个对应rviz中planning的plan按键，只会规划路径，可以在界面中看到规划的路径，但是并不会让机器人开始运动，如果要让机器人运动，需要使用execute(my_plan)，对应execute按键。当然，我们也可以使用一个move()来代替paln()和execute()。具体的API说明，可以参考官方的文档：

http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html

$ roslaunch seven_dof_arm_config demo.launch

$ rosrun seven_dof_arm_test test_custom_node

四、碰撞检测

moveit可以帮助我们完成机器人的自身碰撞检测和环境障碍物碰撞检测，rivz的GUI中，我们可以通过 Planning Scene插件来导入障碍物并且对障碍物进行编辑，现在我们在前边学习内容的基础上，通过代码加入一个障碍物，看下会对运动规划有什么影响。

// 包含API的头文件

#include <moveit/move_group_interface/move_group.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/AttachedCollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

int main(int argc, char **argv)

{

ros::init(argc, argv, "add_collision_objct");

ros::NodeHandle nh;

ros::AsyncSpinner spin(1);

spin.start();

// 创建运动规划的情景，等待创建完成

moveit::planning_interface::PlanningSceneInterface current_scene;

sleep(5.0);

// 声明一个障碍物的实例，并且为其设置一个id，方便对其进行操作，该实例会发布到当前的情景实例中

moveit_msgs::CollisionObject cylinder;

cylinder.id = "seven_dof_arm_cylinder";

// 设置障碍物的外形、尺寸等属性

shape_msgs::SolidPrimitive primitive;

primitive.type = primitive.CYLINDER;

primitive.dimensions.resize(3);

primitive.dimensions[0] = 0.6;

primitive.dimensions[1] = 0.2;

// 设置障碍物的位置

geometry_msgs::Pose pose;

pose.orientation.w = 1.0;

pose.position.x = 0.0;

pose.position.y = -0.4;

pose.position.z = 0.4;

// 将障碍物的属性、位置加入到障碍物的实例中

cylinder.primitives.push_back(primitive);

cylinder.primitive_poses.push_back(pose);

cylinder.operation = cylinder.ADD;

// 创建一个障碍物的列表，把之前创建的障碍物实例加入其中

std::vector<moveit_msgs::CollisionObject> collision_objects;

collision_objects.push_back(cylinder);

// 所有障碍物加入列表后（这里只有一个障碍物），再把障碍物加入到当前的情景中，如果要删除障碍物，使用removeCollisionObjects(collision_objects)

current_scene.addCollisionObjects(collision_objects);

ros::shutdown();

return 0;

}

我们再来编译运行一下，看看会发生什么。

$ roslaunch seven_dof_arm_config demo.launch

$ rosrun seven_dof_arm_test add_collision_objct

稍等五秒钟，有没有看到，现在界面中多了一个圆柱体。

在scene objects中可以对障碍物的属性进行再次调整。

障碍物加入之后，再运动机械人的时候，就会检测机器人是否会与障碍物发生碰撞，比如我们用鼠标拖动机器人的终端，让机器人和障碍物发生碰撞：

有没有看到机械臂的部分links变成了红色，这就说明这些links和障碍物发生了碰撞，如果此时进行运动规划，会提示错误的。

上面的代码只是在场景中加入了障碍物，碰撞检测由moveit的插件完成，那么我们如何通过代码来检测是否发生了碰撞呢？可以学习源码包中的 check_collision.cpp：

#include <ros/ros.h>

// 包含moveit的API

#include <moveit/robot_model_loader/robot_model_loader.h>

#include <moveit/planning_scene/planning_scene.h>

#include <moveit/kinematic_constraints/utils.h>

#include <eigen_conversions/eigen_msg.h>

int main(int argc, char **argv)

{

ros::init (argc, argv, "arm_kinematics");

ros::AsyncSpinner spinner(1);

spinner.start();

// 加载机器人的运动学模型到情景实例中

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");

robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

planning_scene::PlanningScene planning_scene(kinematic_model);

// 自身碰撞检测

// 首先需要创建一个碰撞检测的请求对象和响应对象，然后调用碰撞检测的API checkSelfCollision，检测结果会放到collision_result中

collision_detection::CollisionRequest collision_request;

collision_detection::CollisionResult collision_result;

planning_scene.checkSelfCollision(collision_request, collision_result);

ROS_INFO_STREAM("1. Self collision Test: "<< (collision_result.collision ? "in" : "not in")

<< " self collision");

// 修改机器人的状态

// 我们可以使用场景实例的getCurrentStateNonConst()获取当前机器人的状态，然后修改机器人的状态到一个随机的位置，

// 清零collision_result的结果后再次检测机器人是否发生滋生碰撞

robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();

current_state.setToRandomPositions();

collision_result.clear();

planning_scene.checkSelfCollision(collision_request, collision_result);

ROS_INFO_STREAM("2. Self collision Test(Change the state): "<< (collision_result.collision ? "in" : "not in"));

// 我们也可以指定查询一个group是否和其他部分发生碰撞，只需要在collision_request中修改group_name属性

collision_request.group_name = "arm";

current_state.setToRandomPositions();

collision_result.clear();

planning_scene.checkSelfCollision(collision_request, collision_result);

ROS_INFO_STREAM("3. Self collision Test(In a group): "<< (collision_result.collision ? "in" : "not in"));

// 获取碰撞关系

// 首先，我们先让机器人发生自身碰撞

std::vector<double> joint_values;

const robot_model::JointModelGroup* joint_model_group =

current_state.getJointModelGroup("arm");

current_state.copyJointGroupPositions(joint_model_group, joint_values);

joint_values[2] = 1.57; //原来的代码这里是joint_values[0]，并不会导致碰撞，我改成了joint_values[2]，在该状态下机器人会发生碰撞

current_state.setJointGroupPositions(joint_model_group, joint_values);

ROS_INFO_STREAM("4. Collision points "

<< (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));

// 然后我们再来检测机器人是否发生了自身碰撞，已经发生碰撞的是哪两个部分

collision_request.contacts = true;

collision_request.max_contacts = 1000;

collision_result.clear();

planning_scene.checkSelfCollision(collision_request, collision_result);

ROS_INFO_STREAM("5. Self collision Test: "<< (collision_result.collision ? "in" : "not in")

<< " self collision");

collision_detection::CollisionResult::ContactMap::const_iterator it;

for(it = collision_result.contacts.begin();

it != collision_result.contacts.end();

++it)

{

ROS_INFO("6 . Contact between: %s and %s",

it->first.first.c_str(),

it->first.second.c_str());

}

// 修改允许碰撞矩阵（Allowed Collision Matrix，acm)

// 我们可以通过修改acm来指定机器人是否检测自身碰撞和与障碍物的碰撞，在不检测的状态下，即使发生碰撞，检测结果也会显示未发生碰撞

collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();

robot_state::RobotState copied_state = planning_scene.getCurrentState();

collision_detection::CollisionResult::ContactMap::const_iterator it2;

for(it2 = collision_result.contacts.begin();

it2 != collision_result.contacts.end();

++it2)

{

acm.setEntry(it2->first.first, it2->first.second, true);

}

collision_result.clear();

planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);

ROS_INFO_STREAM("6. Self collision Test after modified ACM: "<< (collision_result.collision ? "in" : "not in")

<< " self collision");

// 完整的碰撞检测

// 同时检测自身碰撞和与障碍物的碰撞

collision_result.clear();

planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);

ROS_INFO_STREAM("6. Full collision Test: "<< (collision_result.collision ? "in" : "not in")

<< " collision");

ros::shutdown();

return 0;

}

编译运行：

$ roslaunch seven_dof_arm_config demo.launch

$ roslaunch seven_dof_arm_test check_collision

可以看到碰撞检测的结果：

更多MoveIt API的使用例程还可以参考下边的链接：

http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html

还有MoveIt interface的Class文档：

http://docs.ros.org/indigo/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html

（二十七）——ROS Industrial

工业机器人是机器人中非常重要的一个部分，在工业领域应用广泛而且成熟，ROS迅猛发展的过程中，也不断渗入到工业领域，从而产生了一个新的分支——ROS-Industrial（ROS-I）。

ROS-I的官网：http://rosindustrial.org/

一、ROS-I的目标

将ROS强大的功能应用到工业生产的过程中；

为工业机器人的研究与应用提供快捷有效的开发途径；

为工业机器人创建一个强大的社区支持；

为工业机器人提供一站式的工业级ROS应用开发支持。

ROS向工业领域的渗透，可以将ROS中丰富的功能、特性带给工业机器人，比如运动规划，运动学算法，视觉感知，还有rviz，gazebo等工具，不仅降低了原本复杂严格的工业机器人研发门槛，而且在研发成本发面也具有极大的优势。

二、ROS-I的安装

在完整安装ROS之后，通过以下的命令就可以安装ROS-I了：

$ sudo apt-get install ros-indigo-industrial-core ros-indigo-open-industrial-ros-controllers

三、ROS-I的架构

GUI：上层UI分为两个部分：一个部分是ROS中现在已有的UI工具；另外一个部分是专门针对工业机器人通用的UI工具，不过是将来才会实现。

ROS Layer：ROS基础框架，提供核心通讯机制

MoveIt! Layer：为工业机器人提供规划、运动学等核心功能的解决方案

ROS-I Application Layer：处理工业生产的具体应用，也是针对将来的规划

ROS-I Interface Layer：接口层，包括工业机器人的客户端，可以通过 simple message协议与机器人的控制器通信

ROS-I Simple Message Layer：通信层，定义了通信的协议，打包和解析通信数据

ROS-I Controller Layer：机器人厂商开发的工业机器人控制器。

从上边的架构我们可以看到，ROS-I在复用已有ROS框架、功能的基础上，针对工业领域进行了针对性的拓展，而且可以通用于不同厂家的机器人控制器。

四、ROS-I控制UR机械臂

Universal Robots是丹麦的一家工业机器人制作商，主要的机器人产品有：UR3、UR5和UR10，分别针对不同的负载：

我们以该机器人作为示例，看一下ROS-I的应用。

4.1 安装

首先我们需要安装UR机器人的ROS功能包集。

$ sudo apt-get install ros-indigo-universal-robot

该功能包集包含了UR仿真、运行需要的主要功能包：ur_description， ur_driver， ur_bringup， ur_gazebo， ur_msgs， ur10_moveit_config/ur5_moveit_config， ur_kinematics。

4.2 运行

安装完成后，使用下边的命令，我们就可以看到UR10的机器人模型了（第一次运行需要等待较长时间完成模型加载）：

$ roslaunch ur_gazebo ur10.launch

然后我们让机械臂动起来，需要运行MoveIt运动规划的节点：

$ roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch sim:=true

然后运行rviz：

$ roslaunch ur10_moveit_config moveit_rviz.launch config:=true

启动之后，我们可以看到rivz中的机器人模型和gazebo中的机器人模型应该是一样的姿态。在rviz中，我们可以用鼠标拖动机器人的终端，然后点击planning标签页中的plan nad execute就可以让机器人规划路径并且到达目标位置了，gazebo中的模型也会跟随变化。

4.3 分析

看到这里，也许我们会有一个疑问：这不就是MoveIt那一套东西么，感觉和ROS-I并没有什么关系？再回头看ROS-I的架构，这种架构的上层控制本身就是复用的已有的软件包，ROS-I目前主要关注的是如何使用这些软件包来控制工业机械臂，也就是最下边的三层结构。我们把这三层从上到下分析一下：

首先是ROS-I Interface Layer层，这一层需要我们设计一个机器人的客户端节点，主要功能是完成数据从ROS到机械臂的转发，ROS-I为我们提供了许多编程接口，可以帮助我们快速开发，下图就是几个比较常用的API，具体API的使用说明可以查看官方文档。

对于机械臂来讲，这里最重要的是 robot_state和 joint_trajectory。 robot_state包括很多状态信息，ROS-I都已经帮我们定义好了，可以去industrial_msgs包里看到消息的定义文件。joint_trajectory订阅了MoveIt规划出来的路径消息，然后打包发送给最下层的机器人服务器端。通常会把这一层的功能封装成robot_name_driver功能包，可以看ROS-I中ABB和UR的机械臂都是这样的，可以参考他们的源码进行设计。

然后是ROS-I Simple Message Layer层，这一层主要是上下两层的通信协议。Simple Message这个协议是基于TCP的，上下层客户端和服务器端的消息交互，全部通过这一层提供的API进行打包和解析。具体使用方法可以参考，也可以直接看ROS-I的源码：，主要实现SimpleSerialize和TypedMessage两个类的功能即可。

最下层的ROS-I Controller Layer是厂家自己的控制器，考虑到实时性的要求，一般不会使用ROS，只要留出TCP的接口即可，接收到trajectory消息解析之后，就按照厂家自己的算法完成动作了。

可见，如果我们想要通过ROS-I来控制自己的机械臂，最下边的三层使我们需要实现的重点，上层运动规划部分可以交给ROS来完成。

（二十八）——机器听觉

机器人通过机器视觉看到色彩斑斓的世界，但是人类最美好的不只是看到的，还有听到的，让机器人听懂人类的语音，同样是一样非常美妙的事情。机器听觉，简单来说就是让机器人能听懂人说的话，以便更好的服务于人类。将语音——人类最自然的沟通和交换信息的媒介应用到智能机器人控制中，在机器人系统上增加语音接口，用语音代替键盘输入，并进行人机对话，不仅是将语音识别从理论转化为实用的有效证明，同时也是机器人智能化的重要标志之一。

一、语音识别原理

语音识别分为两步：

第一步是根据识别系统的类型选择能够满足要求的一种识别方法，采用语音分析方法分析出这种识别方法所要求额度语音特征参数，这些参数作为标准模式存储起来，这一过程称为“学习”或“训练”。

第二步就是“识别”或“检测”阶段。根据实际需要选择语音特征参数，这些特征参数的时间序列构成了测试模版，将其与已存在的参考模版逐一进行比较，进行测度估计，最后经由专家知识库判决，最佳匹配的参考模版即为识别结果。

原理图如下：

ROS中集成了CMU Sphinx和Festival开源项目中的代码，发布了独立的语音识别功能包。

安装pocketsphinx功能包

首先，我们需要安装pocketsphinx功能包和其依赖的其他声音功能库。

sudo apt-get install gstreamer0.10-pocketsphinx

sudo apt-get install gstreamer0.10-gconf

sudo apt-get install ros-indigo-pocketsphinx

sudo apt-get install ros-indigo-audio-common

sudo apt-get install libasound2

该功能包中的核心节点是recognizer.py文件。这个文件通过麦克风收集语音信息，然后调用语音识别库进行识别生成文本信息，通过/recognizer/output消息发布，其他节点就可以订阅该消息然后进行相应的处理了。

安装完成后我们就可以运行测试了。

首先，插入你的麦克风设备，然后在系统设置里测试麦克风是否有语音输入。

然后，运行包中的测试程序:

roslaunch pocketsphinx robocup.launch

此时，在终端中会看到一些加载功能包的信息。尝试说一些指定的语句，当然，必须是英语，例如：bring me the glass，come with me。在运行的终端中，你应该可以看到识别后的文本信息：

该识别后的消息也会通过/recognizer/output话题发布，我们也可以直接看ROS最后发布的结果消息：

rostopic echo /recognizer/output

从之前的原理介绍上，我们语音识别是将输入的语音与模版进行对比。pocketsphinx

功能包是一种离线的语音识别功能，默认支持的模版有限，我们可以通过下面的命令来查看暂时支持的所有语音指令：

roscd pocketsphinx/demo

more robocup.corpus

如果输入其他模版中不存在的语音信息，语音识别只能匹配最为接近的模版并输出。当然，你会觉得这些模版无法满足你的需求，在下一节中我们会学习如何添加自己需要的语音模版。

语音库中的可识别信息使用txt文档存储，使用如下命令查看功能包中设计的语音指令：

roscd rbx1_speech/config

more nav_commands.txt

你应该可以看到如下可识别的指令：

你可以根据需求，对以上文件进行修改和添加。

然后我们要把这个文件在线生成语音信息和库文件，这一步需要登陆网站http://www.speech.cs.cmu.edu/tools/lmtool-new.html，根据网站的提示上传文件，然后在线编译生成库文件。

点击“选择文件”，然后选择nav_commands.txt文件，再点击“Compile Knowledge Base”按键进行编译。编译完成后，下载“COMPRESSED TARBALL”压缩文件，解压至config文件夹下，这些解压出来的”.dic”和“.lm”文件就是根据我们设计的语音识别指令生成的语音模版库。我们可以给这些文件改个名字：

roscd rbx1_speech/config

rename -f 's/3026/nav_commands/' *

在rbx1_speech/launch文件夹下看看voice_nav_commands.launch这个文件：

</node>

</launch>

可以看到，这个launch文件在运行recognizer.py节点的时候使用了我们生成的语音识别库和文件参数，这样就可以实用我们自己的语音库来进行语音识别了。

通过之前的命令来测试一下效果如何吧：

roslaunch rbx1_speech voice_nav_commands.launch

rostopic echo /recognizer/output

、让机器人说话

现在机器人已经可以按照我们说的话行动了，要是机器人可以和我们对话就更好了。再之前的安装过程中，ros-indigo-audio-common元功能包已经包含了文本转语音的（Text-to-speech，TTS）的功能包sound_play。如果你还没有安装，可以使用下边的命令进行安装：

sudo apt-get install ros-indigo-audio-common

sudo apt-get install libasound2

然后我们来测试一下。在一个终端中运行sound_play的主节点：

rosrun sound_play soundplay_node.py

在另外一个终端中输入需要转化成语音的文本信息：

rosrun sound_play say.py "Greetings Humans. Take me to your leader."

有没有听见声音！ROS通过识别我们输入的文本，让机器人读了出来。发出这个声音的人叫做kal_diphone，如果不喜欢，我们也可以换一个人来读：

sudo apt-get install festvox-don

rosrun sound_play say.py "Welcome to the future" voice_don_diphone

、与机器人对话

接下来我们再玩点更高级的，综合使用前边学习的pocketsphinx和sound_play功能包，再加入一点简单的人工智能，让机器人具备简单的自然语言理解能力，能够和我们进行简单的交流，就像苹果手机上的Siri助手一样。

人工智能标记语言——AIML

AIML（Artificial Intelligence Markup Language，人工智能标记语言）是一种创建自然语言软件代理的XML语言，由Richard Wallace和世界各地的自由软件社区在1995年至2002年发明。AIML主要用于实现机器人的语言交流功能，用户可以与机器人说话，而机器人通过一个自然语言的软件代理，也可以给出一个聪明的回答。目前AIML已经有了Java，Ruby，Python， C ，C#，Pascal等语言的版本。

AIML文件包含一系列已定义的标签。我们通过一个简单的实例学习一下AIML的语法规则。

</category>

</aiml>

1. <aiml>标签：所有的aiml代码都需要介于<aiml>和</aiml>标签之间，该标签包含文件的版本号和编码格式。

2. <category>标签：表示一个基本的知识块，包含一条输入语句和一条输出语句，用来匹配机器人和人交流过程中的一问一答和一问多种应答，但不允许多种提问匹配。

3. <pattern>标签：表示用户的输入语句的匹配，在上边的例子中，用户一旦输入 “How are you” ，机器人就会找到这个匹配。注意，<pattern>标签内的语句必须大写。

4. <template>标签：表示机器人应答语句，机器人找到相应的匹配语句之后，会输出匹配语句对应的应答语句。

有了这几个简单的元素理论上就可以写出任意匹配模式，达到一定智能，但实际应用当中只有这些元素是不够的，我们在通过另一个示例略微深入的理解一下AIML。

<pattern> WHAT IS A ROBOT? </pattern>

A ROBOT IS A MACHINE MAINLY DESIGNED FOR EXECUTING REPEATED TASK WITH SPEED AND PRECISION.

</template>

</category>

</template>

</category>

</aiml>

1. <star/>标签：表示*，这里pattern元素里的匹配模式是用*号表示任意匹配的，但在其他元素里面不能用*号，而用<star/>这个元素来表示。在该示例中，当用户输入“Do you know what a robot is?”后，机器人会使用*匹配输入的“robot”，然后将<star/>替换为“robot”。

2. <srai>标签：表示<srai>里面的话会被当作是用户输入，从新查找匹配模式，直到找到非<srai>定义的回复。用户输入 “Do you know what a robot is?”后，机器人会把“what is a robot”作为用户输入，然后查找到匹配的输出是“A ROBOT IS A MACHINE MAINLY DESIGNED FOR EXECUTING REPEATED TASK WITH SPEED AND PRECISION.”

当然，AIML支持的标签和用法远远不止这些，这里只作为背景知识进行了简单的介绍，如果你想深入的了解、学习AIML，可以访问网站http://www.alicebot.org/aiml.html。

中的AIML解析器

Python有针对AIML的开源解析模块—— PyAIML，该模块可以通过扫描AIML文件，建立一个定向模式树，然后通过深度搜索来匹配用户的输入。我们会使用该模块解析AIML文件，构建我们的机器人AI平台，所以先对该模块进行简单的介绍。

在ubuntu14.04上安装的方法很简单，一句话搞定：

sudo apt-get install python-aiml

想要确定PyAIML是否安装成功，在python终端中输入“import aiml”，如果没有初相任何错误，则安装成功。

>>> import aiml

aiml模块中最重要的类是Kernel()，我们必须创建一个aiml.Kernel()对象，来进行对AIML文件的操作。

>>> mybot = aiml.Kernel()

下一步我们来加载一个AIML文件：

>>> mybot.learn('sample.aiml')

如果是加载多个AIML文件，可以在使用下边的命令：

>>> mybot.learn('startup.xml')

</template>

</category>

</aiml>

我们需要出发一条指令，这个命令会把当前路径下的所有aiml文件加载，并且生成模式匹配树。

>>> mybot.respond("load aiml b")

现在系统已经记住了所有的匹配语句，我们尝试出发一条定义的输入语句：

>>> while True: print k.respond(raw_input("> "))

OK，现在你应该可以看到机器人匹配到了我们输入的语句，并且输出了对应的回复：

来和机器人对话吧

OK，如果有兴趣的话，可以把上边两部分的内容连接到一起，就可以完成与机器人的简单对话了！

（二十九）——功夫茶机器人项目总结

终于结束了18届高交会的功夫茶项目展示，总体来讲，展示效果不错，吸引了众多各年龄段的观众，还上了回CCTV的新闻。

在这里做一个阶段性的总结，也是自己这大半年来的所做所得。

今年4月中旬我来到深圳，和其他几个师兄创建了现在的公司——深圳星河智能科技有限公司，主打工业机器人。5月份完成了Ethercat主站部分的开发，6月份使用ROS建立了整套机器人的开发环境和可视化界面，7月份在实体机器人上实现了运动控制，8月份完成了机器人点动、连续运动的开发，9月份正式开始功夫茶机器人项目，并且完成了动作框架的开发，10月份集成了视觉识别并完成了整套动作的编排调试，11月份完成细节优化和错误处理，最终就是高交会上所展示的效果。

公司所有人都不是工业机器人方面出身，在最初阶段对工业机器人几乎没有概念，但是我们利用深圳的众多资源，迅速学习到了大量相关方面的概念，并且付之于行动，虽然水平还很初级，但至少搭建起了一整套系统。在此之中，最大的功臣莫过于ROS，帮助我们迅速完成了系统构建和应用开发。

ROS中提供了众多关于机械臂方面的资源，其中最重要的莫过于MoveIt！轨迹规划在工业机器人中占据举足轻重的地位，但是对于我们这样初登大堂的开发者来说，还缺少众多专业知识，MoveIt中提供了许多轨迹规划方面的算法，可以让我们把注意力集中在系统的搭理上，而不必纠结算法实现。此外，rviz提供了可扩展的可视化界面，针对我们需要的功能开发一些插件，可以快速建立适合需求的控制监控界面。

当然ROS也不是万能的，它主要完成上层需要的功能，底层的控制器核心完全是自主开发，通过ROS-I中的simple message和上层模块通信，实现轨迹插补，日志上报，实时监控，参数配置，实时读写ethetcat总线等功能。

最后，来发表一些对ROS的看法。ROS虽然好用，但是目前的ROS1还只适合于研发，如果没有强大的研发能力，建议还是不要轻易应用于产品。我们在开发过程中，遇到过以下一些坑：

1. roscore会突然挂掉，node会突然挂掉，rviz会突然挂掉，ROS的一切都会挂，系统就go die。。。

2. moveit没办法实现连续运动，也没有点动等基本操作，需要去看moveit的底层代码然后重新组合去实现。

3. ROS没有实时功能，需要自己搭建实时核，开发实时任务

4. ROS资源占用率较大，对计算机的性能要求较高

目前的ROS1，在产品的研发初期，可以借助其完成快速原型开发，验证产品模型，然后在最终产品中，需要逐渐减弱对ROS的依赖，甚至彻底脱离（至少脱离ROS core），如果需要用到ROS功能包中的代码，也要对其有一定了解。

（三十）——3D地图建模

今天正好有时间，尝试了一下3D地图建模，记录一下流程：

一、安装rgbdslam功能包

在工作空间中下载代码并解压：

wget -q http://github.com/felixendres/rgbdslam_v2/archive/indigo.zip

unzip -q indigo.zip

然后回到catkin_ws的目录下，安装依赖：

rosdep install rgbdslam

rosdep update

依赖包安装完成后，就可以开始编译了：

catkin_make

编译成功后，就可以准备建模了。

二、开始3D建模

先把kinect运行起来，然后开始建模。

roslaunch freenect_launch freenect.launch

roslaunch rgbdslam rgbdslam.launch

可以把kinect放在机器人上，让机器人扫描室内的模型，也可以直接用手转动kinect，同样可以进行建模。按空格键可以停止或者开始建模。

三、保存并回看地图

建模完成后，直接在菜单栏中选择保存成点云数据即可，然后就可以通过pcl_ros（http://wiki.ros.org/pcl_ros ）来查看保存的点云地图了：

rosrun pcl_ros pcd_to_pointcloud quicksave.pcd

在rviz中显示pointcloud2数据：

参考资料

视觉SLAM实战（一）：RGB-D SLAM V2： http://www.cnblogs.com/gaoxiang12/p/4462518.html

RGBDSLAMv2：https://github.com/felixendres/rgbdslam_v2

本站仅提供存储服务，所有内容均由用户发布，如发现有害或侵权内容，请点击举报。