ros之MarkerArray使用

ros之MarkerArray使用

使用ros中的marker array来给点云画boundingbox。

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

void ImageCloudFusion::getMarkerArray(const std::vector<CloudType> cluster_cloud_v,
                                      visualization_msgs::MarkerArray& marker_array)
{
  if (cluster_cloud_v.empty())
  {
    std::cerr << "getMarkerArray: input vector is empty!" << std::endl;
    return;
  }
  marker_array.markers.clear();
  visualization_msgs::Marker bbox_marker;
  bbox_marker.header.frame_id = "rslidar";
  bbox_marker.header.stamp = ros::Time::now();
  bbox_marker.ns = "";
  bbox_marker.color.r = 1.0f;
  bbox_marker.color.g = 0.0f;
  bbox_marker.color.b = 0.0f;
  bbox_marker.color.a = 0.2;
  bbox_marker.lifetime = ros::Duration();
  bbox_marker.frame_locked = true;
  bbox_marker.type = visualization_msgs::Marker::CUBE;
  bbox_marker.action = visualization_msgs::Marker::ADD;

  int marker_id = 0;
  for (size_t i = 0; i < cluster_cloud_v.size(); ++i)
  {
    PointType min_pt, max_pt;
    pcl::getMinMax3D(cluster_cloud_v[i], min_pt, max_pt);
    bbox_marker.id = marker_id;
    bbox_marker.pose.position.x = (max_pt.x + min_pt.x) / 2;
    bbox_marker.pose.position.y = (max_pt.y + min_pt.y) / 2;
    bbox_marker.pose.position.z = (max_pt.z + min_pt.z) / 2;
    bbox_marker.scale.x = max_pt.x - min_pt.x;
    bbox_marker.scale.y = max_pt.y - min_pt.y;
    bbox_marker.scale.z = max_pt.z - min_pt.z;
    marker_array.markers.push_back(bbox_marker);
    ++marker_id;
  }

  if (marker_array.markers.size() > max_marker_size_)
  {
    max_marker_size_ = marker_array.markers.size();
  }

  for (size_t i = marker_id; i < max_marker_size_; ++i)
  {
    bbox_marker.id = i;
    bbox_marker.color.a = 0;
    bbox_marker.pose.position.x = 0;
    bbox_marker.pose.position.y = 0;
    bbox_marker.pose.position.z = 0;
    bbox_marker.scale.x = 0;
    bbox_marker.scale.y = 0;
    bbox_marker.scale.z = 0;
    marker_array.markers.push_back(bbox_marker);
    ++marker_id;
  }
}

注：在publish marker array时， 如果下一次的marker size 小于上一次的，那么会保留上一次的部分mark。所以 在上面各种定义了max_marker_size，将那多余的部分marker设为透明，同时scale为0；