Adding the collision object "box" from darknet_ros to moveit-rviz

edit

Hi everyone,

I was trying to add a bounding box from the detected objects from darknet_ros. the bounding box topic is publish from darknet_ros node as "/darknet_ros_3d/bounding_boxes" This bounding boxes are combinated with point cloud information to calculate (xmin, ymin, zmin) and (xmax, ymax, zmax) 3D coordinates.

So I subscribe to that topic, and would like to add that bounding box as collision object to moveit-rviz. As I want to do collision detection , so need to add the detected objects from darknet_ros as bounding box in moveit-rviz planning scene. I create the code and compile without error but the objects are not shown in moveit-rviz.

Please, can tell me what is the problem?

This is the code:

  #include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/MarkerArray.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/common.h>
#include <pcl/common/projection_matrix.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "moveit/move_group_interface/move_group_interface.h"
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/PlanningScene.h>
#include <geometric_shapes/shape_operations.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_ros/point_cloud.h>
#include <Eigen/Core>
#include <vector>
#include <thread>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/cloud_viewer.h>


ros::Publisher pub;

class AddBoundingBox
{
public:
  AddBoundingBox() 
  {
     ros::NodeHandle nh;
     //add_collision_object_pub = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1000);
     ros::Subscriber sub = nh.subscribe("/darknet_ros_3d_markers", 1, &AddBoundingBox::cloudCB, this);
    ros::spin();
  }


   void addBox()
       {

        //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_new(new pcl::PointCloud<pcl::PointXYZRGB pcl_point(point.x, point.y, point.z>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_new(new pcl::PointCloud<pcl::PointXYZRGB> ());
        pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;

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

        // Define a collision object ROS message.
        moveit_msgs::CollisionObject collision_object;
        //collision_object.header.frame_id = "zed_left_camera_optical_frame";
        collision_object.header.frame_id = move_group.getPlanningFrame();
        collision_object.id = "bounding_box";

        // Define a CUBE  with h, w, d which will be added to the world.
        shape_msgs::SolidPrimitive primitive;
        primitive.type = primitive.BOX;
        primitive.dimensions.resize(3);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
        Eigen::Vector4f centroid;
        Eigen::Vector4f min;
        Eigen::Vector4f max;

        //double xmin, xmin, ymin, ymax, zmin, zmax;
        pcl::getMinMax3D (*cloud_cluster_new, min, max);
        /*min[0] = xmin;
        min[1] = ymin;
        min[2] = zmin;
        max[0] = xmax;
        max[1] = ymax;
        min[2] = zmax;*/
        /* Setting width of cube. */
        primitive.dimensions[0] = (min[0]-max[0]);
        /* Setting height of cube. */
        primitive.dimensions[1] = (min[1]-max[1]);
        /* Setting radius of cylinder. */
        primitive.dimensions[1] = (min[2]-max[1]);

        // Define a pose for the cube (specified relative to frame_id).
        geometry_msgs::Pose cube_pose;
        pcl::compute3DCentroid (*cloud_cluster_new, centroid);
        /* Computing and setting quaternion from axis angle representation. */

        // Setting the position of cube.
        //centroid[0] = cube_pose.position.x;
        //centroid[1] = cube_pose.position.y;
        //centroid[2] = cube_pose.position.z;
        cube_pose.position.x = centroid[0];
        cube_pose.position.y = centroid[1];
        cube_pose.position.z = centroid[2];

        feature_extractor.setInputCloud(cloud_cluster_new);
        feature_extractor.compute();
        std::vector <float> moment_of_inertia;
        std::vector <float> ...