C++ 无法在Moveit Rviz中添加多个碰撞对象

我想在Moveit Rviz中添加多个碰撞对象。我用暗色探测到物体（人），并且能够得到物体（人）的边界框。当尝试在“移动”中将其添加为碰撞对象时，它只能添加一个，但如果检测到更多人，则无法在“移动”中将其全部添加为碰撞对象。这是我的密码

using namespace sensor_msgs;
using namespace std;

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d::ConstPtr& msg)
{ 
    cout<<"Entering call back function" <<endl;
    int person_count = 0;

    int inc = 1;
        while (msg->bounding_boxes[0].Class == "person" )
        {

        for(int count=0; count <= person_count; count++)
                {

                    cout<<"Increment variable" <<  count <<endl;

                    const std::string PLANNING_GROUP = "crane_control";
                    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
                    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

                    string str= to_string(count);
                    moveit_msgs::CollisionObject collision_object;
                    collision_object.header.frame_id = "world";
                    collision_object.id = "BOX_person__" + str;
                    cout<<"Person_BB:" << collision_object.id <<endl;

                    shape_msgs::SolidPrimitive primitive;
                    primitive.type = primitive.BOX;
                    primitive.dimensions.resize(3);

                    std::vector<float> array_xdim(inc);
                    std::vector<float> array_ydim(inc);
                    std::vector<float> array_zdim(inc);

                    array_xdim[inc] = msg->bounding_boxes[0].xmax - msg->bounding_boxes[0].xmin;
                    array_ydim[inc] = msg->bounding_boxes[0].ymax - msg->bounding_boxes[0].ymin;
                    array_zdim[inc] = msg->bounding_boxes[0].zmax - msg->bounding_boxes[0].zmin;

                    cout<<"dimx:" <<  array_xdim[inc] <<endl;
                    cout<<"dimy:" <<  array_ydim[inc] <<endl;
                    cout<<"dimz:" <<  array_zdim[inc] <<endl;

                    primitive.dimensions[0] = array_xdim[inc];
                    primitive.dimensions[1] = array_ydim[inc];
                    primitive.dimensions[2] = array_zdim[inc];   

                    geometry_msgs::Pose box_pose;

                    std::vector<float> array_xpos(inc);
                    std::vector<float> array_ypos(inc);
                    std::vector<float> array_zpos(inc);

                    array_xpos[inc] = (msg->bounding_boxes[0].xmax + msg->bounding_boxes[0].xmin)/2;
                    array_ypos[inc] = (msg->bounding_boxes[0].ymax + msg->bounding_boxes[0].ymin)/2;
                    array_zpos[inc] = (msg->bounding_boxes[0].zmax + msg->bounding_boxes[0].zmin)/2;

                    box_pose.position.x = array_xpos[inc];
                    box_pose.position.y = array_ypos[inc];
                    box_pose.position.z = array_zpos[inc];  
                    box_pose.orientation.w = 1.0;

                    cout<<"position_x:" <<  box_pose.position.x <<endl;
                    cout<<"position_y:" <<  box_pose.position.y <<endl;
                    cout<<"position_z:" <<  box_pose.position.z <<endl;

                    collision_object.primitives.push_back(primitive);
                    collision_object.primitive_poses.push_back(box_pose);
                    collision_object.operation = collision_object.ADD;
                    std::vector<moveit_msgs::CollisionObject> collision_objects;
                    collision_objects.push_back(collision_object);
                    planning_scene_interface.applyCollisionObjects(collision_objects);

                    cout<<"count:" <<  count <<endl;

                }
                inc++;
                person_count++;
                cout<<"Person count" << person_count <<endl;
    }
    cout<<"Exiting call back function" <<endl;
}

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", 50, chatterCallback);
  ros::spin();  
 return 0;
}

使用名称空间传感器\u msgs；
使用名称空间std；
void chatterCallback（const darknet_ros_3d_msgs:：BoundingBoxes3d:：ConstPtr&msg）
{ 
库特