Moveit obstacle avoidance path planning demo

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>


int main(int argc, char **argv)
{
    ros::init(argc, argv, "full_demo");
    ros::NodeHandle nh;
    ros::AsyncSpinner spin(1);
    spin.start();

    //     Create a scenario for motion planning , Wait for creation to complete 
    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::MoveGroupInterface arm("arm");
    // Barrier free movement 
    std::vector<double> joints={0.7,-1.57,0,0,0};
    arm.setJointValueTarget(joints);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(my_plan);
    if(success) {
        arm.execute(my_plan);
    }
    joints={-0.7,-1.57,0,0,0};
    arm.setJointValueTarget(joints);
    success = arm.plan(my_plan);
    if(success) {
        arm.execute(my_plan);
    }

    moveit_msgs::PlanningScene planning_scene;
    std::string id_1="1";
    std::string id_2="2";


//    //  The first obstacle 
    moveit_msgs::CollisionObject cylinder;
    cylinder.header.frame_id = "base_link";
    cylinder.id=id_1;
    // Define the shape and size of the object 
    shape_msgs::SolidPrimitive primitive;
    primitive.type=primitive.CYLINDER;
    primitive.dimensions.resize(3); //dimensions It's a vector, Distribution for them 3 Element space 
    primitive.dimensions[0] =0.6;   // Cylinder height 
    primitive.dimensions[1] =0.05;   // radius 

    geometry_msgs::Pose pose;
    pose.orientation.w =1.0;
    pose.position.x=0;
    pose.position.y=0.2;
    pose.position.z=0;
    cylinder.primitives.push_back(primitive);
    cylinder.primitive_poses.push_back(pose);
    // Define the operation as add 
    cylinder.operation = cylinder.ADD;
    planning_scene.world.collision_objects.emplace_back(cylinder);

    // The second obstacle 
    cylinder.header.frame_id = "base_link";
    cylinder.id=id_2;
    // Define the shape and size of the object 
    primitive.type=primitive.CYLINDER;
    primitive.dimensions.resize(3); //dimensions It's a vector, Distribution for them 3 Element space 
    primitive.dimensions[0] =0.3;   // Cylinder height 
    primitive.dimensions[1] =0.05;   // radius 

    pose.orientation.w =1.0;
    pose.position.x=-0.07;
    pose.position.y=0.2;
    pose.position.z=0;
    cylinder.primitives.push_back(primitive);
    cylinder.primitive_poses.push_back(pose);
    cylinder.operation = cylinder.ADD;
    planning_scene.world.collision_objects.emplace_back(cylinder);

    // Release 
    planning_scene.is_diff = true;
    planning_scene_diff_publisher.publish(planning_scene);

    // Obstacle avoidance movement 
    joints={0.7,-1.57,0,0,0};
    arm.setJointValueTarget(joints);
    success = arm.plan(my_plan);
    if(success) {
        arm.execute(my_plan);
    }

    // Remove obstacles 
    moveit::planning_interface::PlanningSceneInterface current_scene;
    std::vector<std::string> object_ids;
    object_ids.emplace_back(id_1);
    object_ids.emplace_back(id_2);
    current_scene.removeCollisionObjects(object_ids);


    while (ros::ok()){
    }

    return 0;
}