How can I stop the Moveit planning execution when there is a collsion between the robot and the object in the scene?

edit

Hi

I have successfully introduced collision objects in the Moveit planning scene. The collision objects are real objects detected with a ZED camera and they are in the Moveit simulation as can see from this code and RVIZ screenshot. Here the collision objects code

using namespace sensor_msgs;
using namespace std;

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 

 int counter_id = 0;
 const std::string PLANNING_GROUP = "crane_control";    
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 std::vector<std::string> object_ids;
 moveit_msgs::CollisionObject collision_object;
 collision_object.header.frame_id = "world";
 collision_object.id = "BOX_";
 std::vector<moveit_msgs::CollisionObject> collision_objects;

          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)-10;
                                  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 objects                                                                                                                               
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);                                                                   
                                  collision_objects.push_back(collision_object); 
                                  sleep(0.5);                                                        

                            }
                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                 planning_scene_interface.removeCollisionObjects(object_ids); 
                                 sleep(0.3); 
                                 collision_object.operation = collision_object.ADD;
                                 planning_scene_interface.applyCollisionObjects(collision_objects);

 }

int main(int argc,  char** argv)

{
  ros::init(argc, argv, "cpp_subscriber");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 5, chatterCallback);
  ros::spin();  

}

Here can see the collision object with RVIZ

So, now first I would like to give the robot a path-goal from point A to B and then when is in the collision with the object just to stop. Any Help?

Thanks