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)
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.
I include the plugin in the following way:
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:
What exactly does this error indicate and how do I need to adjust my code to get rid of it?
Thanks in advance!