最新消息:

ROS探索总结-26.MoveIt编程

ROS1/一代机器人系统 少儿编程 2070浏览 0评论
ROS探索总结

本文转载自古月居,原作者古月,原文链接:https://www.guyuehome.com/455。

MoveIt编程

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

ROS探索总结-26.MoveIt编程

一、创建功能包

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

$ 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中看到机械臂的动作了。

ROS探索总结-26.MoveIt编程

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

接下来我们继续学习如何使用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

效果图:

ROS探索总结-26.MoveIt编程

四、碰撞检测

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

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

ROS探索总结-26.MoveIt编程

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

ROS探索总结-26.MoveIt编程

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

ROS探索总结-26.MoveIt编程

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

ROS探索总结-26.MoveIt编程

上面的代码只是在场景中加入了障碍物,碰撞检测由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

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

ROS探索总结-26.MoveIt编程

更多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

您必须 登录 才能发表评论!