Custom Nav2 Costmap Plugin crashes Action Server

edit

ROS2: Humble Ubuntu: 22.04

Hi all,

I am currently trying to create a custom Nav2 Costmap Plugin. In general, I am a very beginner with ROS2 and C++ programming. So far, the plugin does basically nothing (as far as I think). You can see the code of the plugin below:

namespace nav2_sem_costmap_plugin {

void 
SemLayer::onInitialize() {

  // START Subscription to topic
  auto node = node_.lock();
  obj_sub_ = node->create_subscription<object_msgs::msg::Object>("/object_detection", rclcpp::SensorDataQoS(),std::bind(&SemLayer::objectCallback, this, std::placeholders::_1));
  RCLCPP_INFO(node->get_logger(), "SemLayer: subscribed to " "topic %s", obj_sub_->get_topic_name());
  // END Subscription to topic

  // Whether to apply this plugin or not
  declareParameter("enabled", rclcpp::ParameterValue(true));
  declareParameter("publish_occgrid", rclcpp::ParameterValue(false));
  get_parameters();

  if (publish_occgrid_) {
    grid_pub_ =
        node->create_publisher<nav_msgs::msg::OccupancyGrid>("sem_grid", 1);
    grid_pub_->on_activate();
  }
}

void 
SemLayer::get_parameters() {
  auto node = node_.lock();
  node->get_parameter(name_ + "." + "enabled", enabled_);
  node->get_parameter(name_ + "." + "publish_occgrid", publish_occgrid_);
}

void 
SemLayer::objectCallback(const object_msgs::msg::Object::SharedPtr msg) {
  obj_message_mutex_.lock();
  object = *msg;
  obj_message_mutex_.unlock();
  obj_detected_ = true;
}

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

}

void
SemLayer::onFootprintChanged()
{
  need_recalculation_ = true;
  RCLCPP_DEBUG(rclcpp::get_logger(
      "nav2_costmap_2d"), "SemLayer::onFootprintChanged(): num footprint points: %lu",
    layered_costmap_->getFootprint().size());
}

void 
SemLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid,int min_i, int min_j, int max_i, int max_j) {

  if (!enabled_) {
    return;
  }

  if (!obj_detected_){
    return;
  }

  get_parameters();

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

  obj_detected_ = false;
}

Note: I know that the updateBounds method is empty, but the outcome of the method interferes with the InflationLayer plugin. If I remove the plugin or simply leafe this method empty, everthing works.

I include the plugin in the following way:

local_costmap:
  local_costmap:
    ros__parameters:
      lethal_cost_threshold: 50
      update_frequency: 10.0
      publish_frequency: 10.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: True
      width: 10
      height: 10
      resolution: 0.05
      robot_radius: 0.4
      plugins: ["obstacle_layer", "inflation_layer", "sem_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 0.4
        inflation_radius: 0.32
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      sem_layer:
        plugin: "nav2_sem_costmap_plugin::SemLayer"
        enabled: True
        publish_occgrid: False
      always_send_full_costmap: False
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

So, when I now run everything I get a very cryptic warning:

[controller_server]: [follow_path][ActionServer] Aborting handle.

How is it, that a plugin which does effectively nothing crushes everything so bad? Is it to do with the updateBounds method? And if so, how do I make it so that this and the one from the inflation layer don't get in each other's way?

Many thanks in advance!!