Clearing the whole cost map seems like overkill if all you really want to do is clear your own layer. One option is to disable your layer using dynamic reconfigure, like in the Creating a New Layer tutorial:
#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)
using costmap_2d::LETHAL_OBSTACLE;
namespace simple_layer_namespace
{
SimpleLayer::SimpleLayer() {}
void SimpleLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
current_ = true;
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
&SimpleLayer::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
}
void SimpleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
mark_x_ = robot_x + cos(robot_yaw);
mark_y_ = robot_y + sin(robot_yaw);
*min_x = std::min(*min_x, mark_x_);
*min_y = std::min(*min_y, mark_y_);
*max_x = std::max(*max_x, mark_x_);
*max_y = std::max(*max_y, mark_y_);
}
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
}
} // end namespace
Notice that the enabled_ flag is set in reconfigureCB() and referenced in updateBounds() and updateCosts(). When enabled_ is false, the layer does not modify the costmap.
To actually set it to false, you would use a dynamic_reconfigure client in some separate code/custom node:
#include <ros/ros.h>
#include <dynamic_reconfigure/client.h>
#include <simple_layer/SimpleLayerConfig.h>
layer_client_local_ = new dynamic_reconfigure::Client<SimpleLayerConfig>("move_base/local_costmap/simple_layer");
SimpleLayerConfig layer_config;
layer_client_local_->getCurrentConfiguration(layer_config, ros::Duration(1.0));
layer_config.enabled = false;
layer_client_local_->setConfiguration(layer_config);
This code is untested, but I hope it gives you an idea of how to proceed.
Originally posted by tryan with karma: 1421 on 2020-11-16
This answer was NOT ACCEPTED on the original site
Post score: 1
Original comments
Comment by geoporus on 2020-11-17:
Hello @tryan, thanks for the reply! While I believe your answer lead me to looking into some valuable piece of info regarding ROS development, I am not quite sure it is exactly what I need. The possibility of dynamically disabling a layer with dynamic reconfigure is really cool and I might make goof use of it in the future for my project but I don't think this solves my issue now. Here are some key points as to why I think so and also some more details about my problem.
- With what I am trying to do, I do not want to enable/disable the whole
virtual_walls layer from modifying the costmap. I just want that when I remove certain points from the PointCloud in that custom layer, the modifications that those PointClouds initially brought to the costmaps would also disappear. Think about this scenario: in my virtual_walls layer I have a line obstacle and a circle obstacle. I want that when I remove one of them, that specific obstacle also disappears from the costmaps as well.
Comment by geoporus on 2020-11-17:
The expected behavior that I am describing in the comment above happens when I define a custom recovery behavior for just the custom obstacles layer that I have, like this:
recovery_behaviour_enabled: true
recovery_behaviors:
- name: 'obstacles_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
obstacles_reset:
reset_distance: 0.0
layer_names: ["virtual_walls_layer"]
My issue is now that this behavior gets called only when I have such a ghost obstacle and I am publishing a Goal for move_base so that it goes into the recovery behavior. So, i was wondering whether I could call this recovery behavior somehow manually, just for my specific layer, from inside my package developed in Python. The recovery behavior only provides a plugin ROS API in c++ and I am not sure how to call that from Python.
I would add some RVIZ screenshots for better understanding of the situation but I cannot due to not having enough karma points .
Thanks!
Comment by tryan on 2020-11-17:
Thank you for the clarification. I still think recovery behavior sounds like a workaround rather than the best solution; you can implement the desired behavior in the layer's updateCosts() function, including clearing/redrawing the layer based on current information as you see fit. Layers above it should then update accordingly. You can trigger the layer's behavior in other code using a Python dynamic_reconfigure client if necessary.
In any case, to my knowledge, move_base doesn't expose recovery behavior plugin functionality (here's where they're used in move_base.cpp), so you won't be able to call it from elsewhere. Hopefully, someone will correct me if I'm wrong. If I'm still misunderstanding your use case, I apologize.
