marcs's profile - activity

I have a node which publishes a custom message (BoundingBox3d.msg) displaying the coordinates of a detected human,

string Class
float64 probability
float64 x
float64 y
float64 z

and another custom message (BoundingBoxes3d.msg) with that array

std_msgs/Header header
BoundingBox3d[] bounding_boxes

However i would require it to be published as a posearray message type, because another node requires the input to be of type geometry_msgs/PoseArray. The node (Darknet3D.cpp) produces the output values x, y and z and I have been trying unsuccessfully to publish the values into a different message type. All the comments with //M Test are the additional code written by me.

When I rostopic /posearray, the values of the posearray are all blank.

header: 
  seq: 682
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
poses: []

Could anyone point out why I am not able to get values for my posearray? I am pretty new to coding so all my attempts have been comparing different open source codes and improvising for my needs. I tried using ROS_INFO("poseArray size: %i", posearray.poses.size()) to see if the size of the posearray, but it remains as 0. The other pose.x etc values all returns correct values. Below is the cpp file which outputs the values

#include "darknet_ros_3d/Darknet3D.h"

#include <ros/ros.h>

#include <visualization_msgs/MarkerArray.h>

#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <limits>
#include <algorithm>

#include "geometry_msgs/Pose.h"
#include <geometry_msgs/PoseStamped.h> /*M test*/
#include <geometry_msgs/PoseArray.h> /*M test*/

#include "ros/time.h"


namespace darknet_ros_3d
{

Darknet3D::Darknet3D():
  nh_("~")
{
  initParams();

  darknet3d_pub_ = nh_.advertise<darknet_ros_3d_msgs::BoundingBoxes3d>(output_bbx3d_topic_, 100);
  markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/darknet_ros_3d/markers", 100);

  posearray_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/posearray", 100); //M Test

  yolo_sub_ = nh_.subscribe(input_bbx_topic_, 1, &Darknet3D::darknetCb, this);
  pointCloud_sub_ = nh_.subscribe(pointcloud_topic_, 1, &Darknet3D::pointCloudCb, this);

  last_detection_ts_ = ros::Time::now() - ros::Duration(60.0);
}


void
Darknet3D::initParams()
{
  input_bbx_topic_ = "/darknet_ros/bounding_boxes";
  output_bbx3d_topic_ = "/darknet_ros_3d/bounding_boxes";
  pointcloud_topic_ = "/r200/camera/depth_registered/points";
  working_frame_ = "odom";/*rs200_camera*/
  mininum_detection_thereshold_ = 0.5f;
  minimum_probability_ = 0.3f;

  nh_.param("darknet_ros_topic", input_bbx_topic_, input_bbx_topic_);
  nh_.param("output_bbx3d_topic", output_bbx3d_topic_, output_bbx3d_topic_);
  nh_.param("point_cloud_topic", pointcloud_topic_, pointcloud_topic_);
  nh_.param("working_frame", working_frame_, working_frame_);
  nh_.param("mininum_detection_thereshold", mininum_detection_thereshold_, mininum_detection_thereshold_);
  nh_.param("minimum_probability", minimum_probability_, minimum_probability_);
  nh_.param("interested_classes", interested_classes_, interested_classes_);


}

void
Darknet3D::pointCloudCb(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  point_cloud_ = *msg;
}

void
Darknet3D::darknetCb(const darknet_ros_msgs::BoundingBoxes::ConstPtr& msg)
{
  last_detection_ts_ = ros::Time::now();
  original_bboxes_ = msg->bounding_boxes;
}

void
Darknet3D::calculate_boxes(const sensor_msgs::PointCloud2& cloud_pc2,
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_pcl,
    darknet_ros_3d_msgs::BoundingBoxes3d* boxes)
{
  boxes->header.stamp = cloud_pc2.header.stamp;
  boxes->header.frame_id = working_frame_;

  for (auto bbx : original_bboxes_)
  {
    if ((bbx.probability < minimum_probability_) ||
        (std::find(interested_classes_.begin(), interested_classes_.end(), bbx.Class) == interested_classes_.end()))
    {
      continue;
    }

    int center_x, center_y;

    center_x = (bbx.xmax + bbx.xmin) / 2;
    center_y = (bbx.ymax + bbx.ymin) / 2;

    int pcl_index = (center_y* cloud_pc2.width) + center_x;
    pcl::PointXYZRGB center_point =  cloud_pcl->at(pcl_index);

    if (std::isnan(center_point.x))
      continue;

    float maxx, minx, maxy, miny, maxz, minz;

    maxx = maxy = maxz =  -std::numeric_limits<float>::max();
    minx = miny = minz =  std::numeric_limits<float>::max();

    for (int i = bbx.xmin; i < bbx.xmax; i++)
      for (int j = bbx.ymin; j < bbx.ymax; j ...