How to create point clouds in ROS to perform clustering from a set of points recieved from ROS msg?

asked 2020-04-13 23:57:28 -0500

Astronaut gravatar image

updated 2020-04-13 23:58:23 -0500

Hi

I have create a node that subscribe to ROS msg (to darknet_ros bounding box msg) and receive a the coordinate points of the box. I like to use this 8 points to create a Centroid and perform clustering with region growing algorithm in order to get the orientation of the object(quaternion). Here is my ROS node to get the Box coordinate

#include "ros/ros.h"
#include "darknet_ros_3d_msgs/BoundingBoxes3d.h"
#include "darknet_ros_3d_msgs/BoundingBox3d.h"
#include "darknet_ros_msgs/BoundingBoxes.h"
#include "darknet_ros_msgs/BoundingBox.h"
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float64MultiArray.h>
#include "geometric_shapes/shape_operations.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <stdlib.h>
#include <iostream>
#include <string>
using namespace std;

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d::ConstPtr& msg)
    { 
            if (!msg->bounding_boxes.empty())
                     {
                        int inc = 1;
                    if(msg->bounding_boxes[0].Class == "box")
                        {
                            float *array_xmin = new float[inc];
                            float *array_xmax = new float[inc];
                            float *array_ymin = new float[inc];
                                 float *array_ymax = new float[inc];
                            float *array_zmin = new float[inc];
                            float *array_zmax = new float[inc];

                            array_xmin[inc] = msg->bounding_boxes[0].xmin; 
                            array_xmax[inc] = msg->bounding_boxes[0].xmax; 
                            array_ymin[inc] = msg->bounding_boxes[0].ymin;
                            array_ymax[inc] = msg->bounding_boxes[0].ymax; 
                            array_zmin[inc] = msg->bounding_boxes[0].zmin; 
                            array_zmax[inc] = msg->bounding_boxes[0].zmax;  

                            inc++;
                            delete[] array_xmin;
                            delete[] array_xmax;
                            delete[] array_ymin;
                            delete[] array_ymax;
                            delete[] array_zmin;
                            delete[] array_zmax;                

                       }            

                    }
    }

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

So how to feed this xmin, xmax, ymin, ymax, zmin, zmax in the point cloud to create Centroid and then segmentation and then get the orientation of the object? Thanks

edit retag flag offensive close merge delete