MoveIt：当机器人与障碍物发生碰撞时，如何使用 move_group 节点进行 pat replnanning？

如何解决MoveIt：当机器人与障碍物发生碰撞时，如何使用 move_group 节点进行 pat replnanning？

嗨，我想在机器人运动期间（在执行轨迹期间）向场景中添加一个碰撞对象。我知道可以使用 move_group 节点，也可以阅读这里的概念 move_group 概念。但是，仍然需要一些帮助来构建节点。我写了 cpp 代码，但认为仍然缺少一些东西，例如 Replanning 复选框和 isPathValid 函数来检查我当前场景中的碰撞，但不确定实现以及我的代码中应该在哪里.到目前为止的代码

using namespace sensor_msgs;
using namespace std;


class ObjectsCollision
{
public:
  const std::string PLANNING_GROUP = "crane_control";   
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  ObjectsCollision()
  {
    ros::NodeHandle n;
    // Initialize subscriber to the raw point cloud
    ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes",50,&ObjectsCollision::cloudCB,this);
    // Spin
    ros::spin();
  }


void cloudCB(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 
    
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
 moveit::planning_interface::MoveGroupInterface::Plan my_plan;
 const std::string MoveGroupInterface = "robot_description";  
  
 int counter_id = 0;
 std::vector<std::string> object_ids;
 moveit_msgs::CollisionObject collision_object;
 //collision_object.header.frame_id = "zed_left_camera_optical_frame";
 collision_object.header.frame_id = move_group.getPlanningFrame();

 collision_object.id = "BOX_";
 std::vector<moveit_msgs::CollisionObject> collision_objects;



  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group.move();


                                 
          for(auto bb : boxes.bounding_boxes)
                    {
                    
                                  shape_msgs::SolidPrimitive primitive;
                                  primitive.type = primitive.BOX;
                                  primitive.dimensions.resize(3);
                                  string str= to_string(counter_id++);                                

                                  //Pose
                                  geometry_msgs::Pose box_pose;
                                  box_pose.position.x = abs ((bb.xmax + bb.xmin)/2);
                                  box_pose.position.y = abs ((bb.ymax + bb.ymin)/2);
                                  box_pose.position.z = abs ((bb.zmax + bb.zmin)/2);
                                  box_pose.orientation.x = 0; 
                                  box_pose.orientation.y = 0;   
                                  box_pose.orientation.z = 0;
                                  box_pose.orientation.w = 1.0;
                                
                                  //Dimension
                                  primitive.dimensions[0] = abs (bb.xmax - bb.xmin);
                                  primitive.dimensions[1] = abs (bb.ymax - bb.ymin);
                                  primitive.dimensions[2] = abs (bb.zmax - bb.zmin); 

                                  //Collision object       
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);                               
                                  collision_objects.push_back(collision_object); 
                                                                    
                                 

                            }
                                 

                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                 planning_scene_interface.removeCollisionObjects(object_ids); 
                                 //sleep(0.6); 
                                 collision_object.operation = collision_object.ADD;

                                 planning_scene_interface.applyCollisionObjects(collision_objects);
                                 //sleep(0.6);
                                
                                move_group.setStartState(*move_group.getCurrentState());
                                geometry_msgs::Pose another_pose;
                                another_pose.orientation.w = 1.0;
                                another_pose.position.x = 0.4;
                                another_pose.position.y = -0.4;
                                another_pose.position.z = 0.9;
                                move_group.setPoseTarget(another_pose);

                                success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
                                    
 }

};


int main(int argc,char** argv)
{
  // Initialize ROS
  ros::init(argc,argv,"collision_object");
  // Start the segmentor
  ObjectsCollision();
}