#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();

    //    创建运动规划的情景，等待创建完成
    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");
    //无障碍的动作
    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";


//    // 第一个障碍物
    moveit_msgs::CollisionObject cylinder;
    cylinder.header.frame_id = "base_link";
    cylinder.id=id_1;
    //定义物体形状尺寸
    shape_msgs::SolidPrimitive primitive;
    primitive.type=primitive.CYLINDER;
    primitive.dimensions.resize(3); //dimensions是一个vector,为其分配3个元素空间
    primitive.dimensions[0] =0.6;   //圆柱体高度
    primitive.dimensions[1] =0.05;   //半径

    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);
    //定义操作为添加
    cylinder.operation = cylinder.ADD;
    planning_scene.world.collision_objects.emplace_back(cylinder);

    //第二个障碍物
    cylinder.header.frame_id = "base_link";
    cylinder.id=id_2;
    //定义物体形状尺寸
    primitive.type=primitive.CYLINDER;
    primitive.dimensions.resize(3); //dimensions是一个vector,为其分配3个元素空间
    primitive.dimensions[0] =0.3;   //圆柱体高度
    primitive.dimensions[1] =0.05;   //半径

    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);

    //发布
    planning_scene.is_diff = true;
    planning_scene_diff_publisher.publish(planning_scene);

    //避障运动
    joints={0.7,-1.57,0,0,0};
    arm.setJointValueTarget(joints);
    success = arm.plan(my_plan);
    if(success) {
        arm.execute(my_plan);
    }

    //移除障碍物
    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;
}