Ask Your Question
0

'No map received' in RVIZ when using a custom costmap2d-layer as plugin

asked 2020-06-03 01:11:18 -0600

Zimba96 gravatar image

Hello there,

I'm having an occupancy grid which has intermediate values from 1-100 and want it to be transformed to a costmap with also intermediate values. I've noticed that this doesn't seem to be possible with the given layers (static layer, inflation layer, obstacle layer, ...)

Therefore, I tried to write a custom layer for which I took the source code of the static_layer and replaced the interpretValue-function in order to map the 100 values of the occupancygrid to the 255 values of the costmap. I integrated my custom plugin into the global_costmap_params.yaml and the system appears to properly load the plugin (at least there are no further errors or warnings that it couldn't be loaded).

The problem is: In RVIZ the global costmap - section throws a warning which says "No map received" (The Topic is '/move_base/global_costmap/costmap' which works fine when static_layer is set as plugin). As a result of that, I can only see the coordinate system, but no map.

I'm using ROS Melodic.

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

Comments

Doesn't answer your direct question, but have you tried setting trinary_costmap to false?

David Lu gravatar image David Lu  ( 2020-06-04 17:03:43 -0600 )edit

I just figured out that the main problem is that my plugins somehow aren't able to read the params from the global_costmap_params.yaml (see my answer). Any ideas on how to fix that? Thanks for your answer btw!

Zimba96 gravatar image Zimba96  ( 2020-06-05 06:33:23 -0600 )edit

Btw, the reason that I wrote this custom plugin was, that the static_map couldn't represent intermediate values by setting the trinary_costmap-param. Turned out that it was the same issue: Instead of using the set trinary_costmap-value "false" of the global_costmap_params.yaml it somehow always used the default-value "true". Manually changing the default value solved the problem..

Zimba96 gravatar image Zimba96  ( 2020-06-05 06:41:04 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-06-05 06:38:03 -0600

Zimba96 gravatar image

Turned out the plugins somehow aren't able to read the given params from global_costmap_params.yaml. Changing the default-value from

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

to

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

solved the issue.

Since this isn't the clean way to do it, any ideas what could be the causes that the plugin doesn't read the params properly?

edit flag offensive delete link more

Comments

I would run rosparam get /move_base and make sure parameters are being loaded where you think they are being loaded. Failing that, post them here.

David Lu gravatar image David Lu  ( 2020-06-05 16:53:44 -0600 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2020-06-03 01:11:18 -0600

Seen: 221 times

Last updated: Jun 05 '20