Ask Your Question
0

Republishing custom message to type /PoseArray

asked 2020-02-20 10:39:09 -0500

marcs gravatar image

updated 2020-02-20 10:57:44 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-02-20 12:26:20 -0500

robustify gravatar image

The posearray message structure in your update() method isn't the same as the local posearray message structure you declare and populate in calculate_boxes(). Instead of declaring a local geometry_msgs::PoseArray in calculate_boxes(), just add a pointer argument like you are already doing with your custom message. Then from update(), pass the pointer to the PoseArray you are actually publishing.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-02-20 10:39:09 -0500

Seen: 26 times

Last updated: Feb 20