Not able to add multiply collision objects in Moveit Rviz

edit

Hi

I would like to add multiply collision objects in Moveit Rviz. I detected the objects (chairs) with darknet_ros and able to get the bounding box of the objects (chairs). When trying to add them as collision objects in Moveit it only able to add one, but I have detected two chairs and want to add two collision objects in the scene. In Rviz Im able to add and visualize only one, but in the terminal output there are two.

Here the code

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 == "chair")
                        {

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


                                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_chair__" + str;
                                cout<<"Chair:" << 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_vector;
                                collision_vector.push_back(collision_object);
                                planning_scene_interface.applyCollisionObjects(collision_vector);
                                cout<<"Chair count" << chair_count <<endl;
                                }
                    chair_count++;
                    inc++;
                    }
              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", 200, chatterCallback);
  ros::spin();  
  return 0;
}

Here the Rviz image

Any help? Thanks