How to create point clouds in ROS to perform clustering from a set of points recieved from ROS msg?
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