zoukankan      html  css  js  c++  java
  • 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;

  • 相关阅读:
    51Nod 2006 飞行员配对(二分图最大匹配)-匈牙利算法
    51Nod 1212无向图最小生成树
    51Nod 1081前缀和
    51Nod 1118 机器人走方格--求逆元
    Java四种线程池的使用
    java常用的几种线程池比较
    一名3年工作经验的程序员应该具备的技能(写得很好,果断转)
    《深入理解mybatis原理》 MyBatis的架构设计以及实例分析
    javadoc
    java 反射机制 观点
  • 原文地址:https://www.cnblogs.com/ChrisCoder/p/10513822.html
Copyright © 2011-2022 走看看