Moveit! 学习记录2——moveit避障

续学习记录1，进行不同规划器的避障效果比较

Moveit!避障实验

通过更换不同的规划器，比较不同路径规划算法的避障效果

过程如下

1.首先在moveitServerCpp.cpp文件下，设置障碍物

在create_table函数中添加和修改即可

我一共设置了两个物体，一个绿色的底板，用于防止机械臂穿模到地下；另外设置了一个障碍物，代码如下

void create_table() {        
        ros::Publisher planning_scene_diff_publisher =     nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

        ros::WallDuration sleep_t(0.5);

        while (planning_scene_diff_publisher.getNumSubscribers() < 1)

        {

         sleep_t.sleep()      
        }

        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

        moveit_msgs::PlanningScene planning_scene;

        moveit_msgs::CollisionObject collision_object;

        collision_object.header.frame_id = arm_->getPlanningFrame();

        // The id of the object is used to identify it.

        collision_object.id = "table";

​

        // Define a box to add to the world.

        shape_msgs::SolidPrimitive primitive;

        primitive.type = primitive.BOX;

        primitive.dimensions.resize(3);

        primitive.dimensions[primitive.BOX_X] = 2;   //长宽2米，高0.01米

        primitive.dimensions[primitive.BOX_Y] = 2;

        primitive.dimensions[primitive.BOX_Z] = 0.01;

​

        // Define a pose for the box (specified relative to frame_id)

        geometry_msgs::Pose box_pose;

        box_pose.orientation.w = 1.0;                    //设置box的位姿

        box_pose.position.x = 0.0;

        box_pose.position.y = 0.0;

        box_pose.position.z = -0.01/2 -0.02;

        collision_object.primitives.push_back(primitive);

        collision_object.primitive_poses.push_back(box_pose);

        collision_object.operation = collision_object.ADD;   //若要放置两个物体，把从这往上到Define a box to add to the world再复制一份

​

        planning_scene.world.collision_objects.push_back(collision_object);

        planning_scene.is_diff = true;

        planning_scene_diff_publisher.publish(planning_scene);​

        ROS_INFO("Added an table into the world");

        // Define a box to add to the world.

        shape_msgs::SolidPrimitive primitive1;

        primitive1.type = primitive1.BOX;

        primitive1.dimensions.resize(3);

        primitive1.dimensions[primitive1.BOX_X] = 0.2;   //长宽2米，高0.01米

        primitive1.dimensions[primitive1.BOX_Y] = 0.2;

        primitive1.dimensions[primitive1.BOX_Z] = 0.2;

​

        // Define a pose for the box (specified relative to frame_id)

        geometry_msgs::Pose box_pose1;

        box_pose1.orientation.w = 1.0;                    //设置box的位姿

        box_pose1.position.x = 0.4;

        box_pose1.position.y = 0.0;

        box_pose1.position.z = -0.01/2 ;

        collision_object.primitives.push_back(primitive1);

        collision_object.primitive_poses.push_back(box_pose1);

        collision_object.operation = collision_object.ADD;   //若要放置两个物体，把从这往上到Define a box to add to the world再复制一份

​

        planning_scene.world.collision_objects.push_back(collision_object);

        planning_scene.is_diff = true;

        planning_scene_diff_publisher.publish(planning_scene);

        ROS_INFO("Added an table into the world");

    }

2.更改cmakelist

add_executable(moveitServerCpp src/moveitServerCpp.cpp)

target_link_libraries(moveitServerCpp ${catkin_LIBRARIES})

然后编译

3.启动模型的rviz看效果

启动后，运行cpp文件可以看到路径规划的避障效果

4.更换不同的规划器

在moveit_control类中更换即可

更换后重新编译再运行

比较不同的避障效果

规划成功

如果规划失败，如下图

 可以更换规划器，如果还不行，就是你设置的那个点确实到不了，换点试试