Hello community,

I need to convert obstacles into polygon shapes and use costmap_converter standalone node. It does convert the obstacles as I can visualize the resulting markers in RVIZ. However the markers are far away off the real obstacle, which I don't understand.

The costmap converter subsrices to costmap and costmap update topic of movebaseflex. In both messages, the frameid is set to "map", which should be taken by costmapconverter standalone node.

Any ideas why frame_id is empty?

This is the callback method of costmapconverter. As you see, it takes the frameid out of incoming message and reuse it.

      void costmapCallback(const nav_msgs::OccupancyGridConstPtr& msg)
  {
      ROS_INFO_ONCE("Got first costmap callback. This message will be printed once");

      if (msg->info.width != map_.getSizeInCellsX() || msg->info.height != map_.getSizeInCellsY() || msg->info.resolution != map_.getResolution())
      {
        ROS_INFO("New map format, resizing and resetting map...");
        map_.resizeMap(msg->info.width, msg->info.height, msg->info.resolution, msg->info.origin.position.x, msg->info.origin.position.y);
      }
      else
      {
        map_.updateOrigin(msg->info.origin.position.x, msg->info.origin.position.y);
      }

  for (std::size_t i=0; i < msg->data.size(); ++i)
  {
    unsigned int mx, my;
    map_.indexToCells((unsigned int)i, mx, my);
    map_.setCost(mx, my, msg->data[i] >= occupied_min_value_ ? 255 : 0 );
  }

  // convert
  converter_->updateCostmap2D();
  converter_->compute();
  costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();

  if (!obstacles)
    return;

  obstacle_pub_.publish(obstacles);

  frame_id_ = msg->header.frame_id;

  publishAsMarker(frame_id_, *obstacles, marker_pub_);

}