Planning is not executing and the collision objects are not in the Moveit scene during executing a trajectoy.

edit

When Im detecting collision object during the execution of the trajectory in the Moveit simulation I got the following error:

[ERROR] [1617248212.709207448]: RRTConnect: Unable to sample any valid states for goal tree

Also, the detected collision objects with the ZED Camera are not shown in the Moveit Scene

Here the 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;
 moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
 moveit::planning_interface::MoveGroupInterface::Plan my_plan;
 const std::string MoveGroupInterface = "robot_description";  

 namespace rvt = rviz_visual_tools;
 moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
 visual_tools.deleteAllMarkers(); 


  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  visual_tools.trigger();

  // Planning to a Pose goal
  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();


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


                            }
                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                 planning_scene_interface.removeCollisionObjects(object_ids); 

                                 sleep(0.5); 
                                 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 the robot controller yaml from the config file

crane:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
    joints:
      - crane_rotation
      - crane_linearX
      - crane_linearZ
      - crane_joint1
      - crane_joint2
      - crane_joint3

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: position_controllers/JointPositionController
    joint: crane_rotation
    # pid: {p: 1000.0, i: 1.0, d: 100.0}
  joint2_position_controller:
    type: position_controllers/JointPositionController
    joint: crane_linearX
    # pid: {p: 500.0, i: 1.0, d: 100.0}
  joint3_position_controller:
    type: position_controllers/JointPositionController
    joint: crane_linearZ
    # pid: {p: 500.0, i: 0.01, d: 10.0}

Here the RVIZ

Any Help?