Error Custom Costmap Plugin - [controller_server]: [follow_path] [ActionServer] Aborting handle

[ChrwagnerTHU]

Hi all,

I am currently trying to write a custom (local) costmap plugin. Unfortunately, I am somewhat lost as to what my mistake or misunderstanding is. So far I'm not actually doing much more than just setting each value of the costmap to 240. You can see the full code below.

void 
SemLayer::onInitialize() {

  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());

  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)
{
  if (need_recalculation_) {
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    // For some reason when I make these -<double>::max() it does not
    // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
    // -<float>::max() instead.
    *min_x = -std::numeric_limits<float>::max();
    *min_y = -std::numeric_limits<float>::max();
    *max_x = std::numeric_limits<float>::max();
    *max_y = std::numeric_limits<float>::max();
    need_recalculation_ = false;
  } else {
    double tmp_min_x = last_min_x_;
    double tmp_min_y = last_min_y_;
    double tmp_max_x = last_max_x_;
    double tmp_max_y = last_max_y_;
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    *min_x = std::min(tmp_min_x, *min_x);
    *min_y = std::min(tmp_min_y, *min_y);
    *max_x = std::max(tmp_max_x, *max_x);
    *max_y = std::max(tmp_max_y, *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;
  }
  // unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
  min_i = std::max(0, min_i);
  min_j = std::max(0, min_j);
  max_i = std::min(static_cast<int>(size_x), max_i);
  max_j = std::min(static_cast<int>(size_y), max_j);
  for (int x = min_i; x < max_i; x++) {
    for (int y = min_j; y < max_j; y++) {
      // int index = master_grid.getIndex(x, y);
      master_grid.setCost(x, y, 240);
    }
}
  obj_detected_ = false;
}
} // namespace nav2_sem_costmap_plugin
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_sem_costmap_plugin::SemLayer, nav2_costmap_2d::Layer)

I include the plugin in the following way:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.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.17
      plugins: ["obstacle_layer", "sem_layer"]
      # plugins: ["obstacle_layer", "inflation_layer"]
      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"
          raytrace_max_range: 8.0
          raytrace_min_range: 0.0
          obstacle_max_range: 9.5
          obstacle_min_range: 0.0
      sem_layer:
        plugin: "nav2_sem_costmap_plugin::SemLayer"
        enabled: True
        publish_occgrid: False
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 0.4
        inflation_radius: 0.32
      always_send_full_costmap: True

When running the navigation, I get the following error: [controller_server]: [follow_path] [ActionServer] Aborting handle.

The corresponding controller server is defined in the following way:

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

What exactly does this error indicate and how do I need to adjust my code to get rid of it?

Thanks in advance!

[SteveMacenski]

Please ask on ros answers or use other costmap layers for reference. This is the wrong venue for questions