navigation - 使用自定义 costmap2d 层作为插件时，RVIZ 中“未收到 map ”

Question

我正在研究 the problem of this thread. 的解决方法

因为现有的 costmap2d 层似乎都不允许使用整个值范围 (0-255)，所以我使用了 the ros-tutorial创建自定义图层。因此，我只使用了 source-code of the static_layer plugin并修改了 interpretValue - 函数，以便将值(这是由于在 -1 和 100 之间使用的占用网格)映射到层的整个范围(应该是 0-255)。我将我的自定义插件集成到 global_costmap_params.yaml 中，系统似乎正确加载了该插件(至少没有进一步的错误或无法加载它的警告)。

问题是:在 RVIZ 中，global costmap - 部分会发出警告，提示“未收到 map ”(主题是“/move_base/global_costmap/costmap”，当设置了 static_layer 时它工作正常作为插件)。结果，我只能看到坐标系，而看不到 map 。

我正在使用 ROS Melodic。

插件源码(occgrid_to_costmap_layer.cpp):

#include<custom_layers/occgrid_to_costmap_layer.h>

#include <ros/ros.h>
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <tf2/LinearMath/Transform.h>
#include <dynamic_reconfigure/server.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <message_filters/subscriber.h>
#include <pluginlib/class_list_macros.h>

 PLUGINLIB_EXPORT_CLASS(occgrid_to_costmap_layer_namespace::OTCLayer, costmap_2d::Layer)


using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;

namespace occgrid_to_costmap_layer_namespace
{

OTCLayer::OTCLayer() : dsrv_(NULL) {}

OTCLayer::~OTCLayer()
{
  if (dsrv_)
    delete dsrv_;
}

void OTCLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_), g_nh;
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();

  std::string map_topic;
  nh.param("map_topic", map_topic, std::string("map"));
  nh.param("first_map_only", first_map_only_, false);
  nh.param("subscribe_to_updates", subscribe_to_updates_, false);

  nh.param("track_unknown_space", track_unknown_space_, true);
  nh.param("use_maximum", use_maximum_, false);

  int temp_lethal_threshold, temp_unknown_cost_value;
  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
  nh.param("trinary_costmap", trinary_costmap_, true);

  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
  unknown_cost_value_ = temp_unknown_cost_value;

  // Only resubscribe if topic has changed
  if (map_sub_.getTopic() != ros::names::resolve(map_topic))
  {
    // we'll subscribe to the latched topic that the map server uses
    ROS_INFO("Requesting the map...");
    map_sub_ = g_nh.subscribe(map_topic, 1, &OTCLayer::incomingMap, this);
    map_received_ = false;
    has_updated_data_ = false;

    ros::Rate r(10);
    while (!map_received_ && g_nh.ok())
    {
      ros::spinOnce();
      r.sleep();
    }

    ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());

    if (subscribe_to_updates_)
    {
      ROS_INFO("Subscribing to updates");
      map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &OTCLayer::incomingUpdate, this);

    }
  }
  else
  {
    has_updated_data_ = true;
  }

  if (dsrv_)
  {
    delete dsrv_;
  }

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &OTCLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}

void OTCLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  if (config.enabled != enabled_)
  {
    enabled_ = config.enabled;
    has_updated_data_ = true;
    x_ = y_ = 0;
    width_ = size_x_;
    height_ = size_y_;
  }
}

void OTCLayer::matchSize()
{
  // If we are using rolling costmap, the static map size is
  //   unrelated to the size of the layered costmap
  if (!layered_costmap_->isRolling())
  {
    Costmap2D* master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
              master->getOriginX(), master->getOriginY());
  }
}

unsigned char OTCLayer::interpretValue(unsigned char value)
{
  if(value==-1){
    return 255;
  }
  else if(value==0){
    return 0;
  }
  else if(value==100){
    return 254;
  }
  else{
    int stepsize = 254/100;
    return int((254-100*stepsize)+stepsize*value);
  }
}

void OTCLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;

  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);

  // resize costmap if size, resolution or origin do not match
  Costmap2D* master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() &&
      (master->getSizeInCellsX() != size_x ||
       master->getSizeInCellsY() != size_y ||
       master->getResolution() != new_map->info.resolution ||
       master->getOriginX() != new_map->info.origin.position.x ||
       master->getOriginY() != new_map->info.origin.position.y))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                                new_map->info.origin.position.y,
                                true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
  }
  else if (size_x_ != size_x || size_y_ != size_y ||
           resolution_ != new_map->info.resolution ||
           origin_x_ != new_map->info.origin.position.x ||
           origin_y_ != new_map->info.origin.position.y)
  {
    // only update the size of the costmap stored locally in this layer
    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    resizeMap(size_x, size_y, new_map->info.resolution,
              new_map->info.origin.position.x, new_map->info.origin.position.y);
  }

  unsigned int index = 0;

  // initialize the costmap with static data
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
  map_frame_ = new_map->header.frame_id;

  // we have a new map, update full size of map
  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  map_received_ = true;
  has_updated_data_ = true;

  // shutdown the map subscrber if firt_map_only_ flag is on
  if (first_map_only_)
  {
    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
    map_sub_.shutdown();
  }
}

void OTCLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
{
  unsigned int di = 0;
  for (unsigned int y = 0; y < update->height ; y++)
  {
    unsigned int index_base = (update->y + y) * size_x_;
    for (unsigned int x = 0; x < update->width ; x++)
    {
      unsigned int index = index_base + x + update->x;
      costmap_[index] = interpretValue(update->data[di++]);
    }
  }
  x_ = update->x;
  y_ = update->y;
  width_ = update->width;
  height_ = update->height;
  has_updated_data_ = true;
}

void OTCLayer::activate()
{
  onInitialize();
}

void OTCLayer::deactivate()
{
  map_sub_.shutdown();
  if (subscribe_to_updates_)
    map_update_sub_.shutdown();
}

void OTCLayer::reset()
{
  if (first_map_only_)
  {
    has_updated_data_ = true;
  }
  else
  {
    onInitialize();
  }
}

void OTCLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                               double* max_x, double* max_y)
{

  if( !layered_costmap_->isRolling() ){
    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
      return;
  }

  useExtraBounds(min_x, min_y, max_x, max_y);

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;
}

void OTCLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!map_received_)
    return;

  if (!enabled_)
    return;

  if (!layered_costmap_->isRolling())
  {
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_)
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
  }
  else
  {
    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame
    geometry_msgs::TransformStamped transform;
    try
    {
      transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
    }
    catch (tf2::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    tf2::Transform tf2_transform;
    tf2::convert(transform.transform, tf2_transform);
    for (unsigned int i = min_i; i < max_i; ++i)
    {
      for (unsigned int j = min_j; j < max_j; ++j)
      {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf2::Vector3 p(wx, wy, 0);
        p = tf2_transform*p;
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my))
        {
          if (!use_maximum_)
            master_grid.setCost(i, j, getCost(mx, my));
          else
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
        }
      }
    }
  }
}

}  // namespace occgrid_to_costmap_layer_namespace

插件 XML (costmap_plugins.xml):

<library path="lib/liboccgrid_to_costmap_layer">
  <class type="occgrid_to_costmap_layer_namespace::OTCLayer" base_class_type="costmap_2d::Layer">
    <description>Layer that maps occupancygrid to costmap with all intermediate values.</description>
  </class>
</library>

CMakeLists.txt:

cmake_minimum_required(VERSION 3.0.2)
project(custom_layers)

 add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  costmap_2d
  dynamic_reconfigure
  map_msgs
  message_filters
  nav_msgs
  roscpp
  pluginlib
  tf2
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(occgrid_to_costmap_layer
  src/occgrid_to_costmap_layer.cpp
)

package.xml:

<?xml version="1.0"?>
<package format="2">
  <name>custom_layers</name>
  <version>0.0.0</version>
  <description>The custom_layers package</description>
  <maintainer email="faps@todo.todo">faps</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>costmap_2d</build_depend>
  <build_depend>dynamic_reconfigure</build_depend>
  <build_depend>map_msgs</build_depend>
  <build_depend>message_filters</build_depend>
  <build_depend>nav_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>pluginlib</build_depend>
  <build_depend>tf2</build_depend>
  <build_export_depend>costmap_2d</build_export_depend>
  <build_export_depend>dynamic_reconfigure</build_export_depend>
  <build_export_depend>map_msgs</build_export_depend>
  <build_export_depend>message_filters</build_export_depend>
  <build_export_depend>nav_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>pluginlib</build_export_depend>
  <build_export_depend>tf2</build_export_depend>
  <exec_depend>costmap_2d</exec_depend>
  <exec_depend>dynamic_reconfigure</exec_depend>
  <exec_depend>map_msgs</exec_depend>
  <exec_depend>message_filters</exec_depend>
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>pluginlib</exec_depend>
  <exec_depend>tf2</exec_depend>

  <export>
    <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
  </export>
</package>

感谢您的帮助!

Answer 1

问题原因同the other thread :

自定义图层无法从 global_costmap_params.yaml 中正确读取参数“map_topic”。 (有什么解决方法吗？)

更改默认参数

nh.param("map_topic", map_topic, std::string("ma​​p"));

到

nh.param("map_topic", map_topic, std::string("insert_required_map_topic_here"));

解决了这个问题。

编辑:

这很可能是由于命名空间问题造成的。为 map_topic-param 指定命名空间(参见上面链接的其他线程)也应该可以解决问题。