How to call move_base's clear_costmap_recovery functionality manually from Python

asked 2020-11-16 08:46:11 -0500

updated 2020-11-17 07:28:59 -0500

Hello everyone!

This is my first question ever on ROS answers so please go easy on me :).

I would like to know how could I manually call the clear_costmap_recovery functionality with a custom obstacles layer from inside a custom package I developed for ROS Melodic on Ubuntu 18.04, in Python.

To give a bit of background to my specific situation, I am working on a project using the ROS 1 navigation stack which required some way of creating "virtual walls/obstacles" for the robot which would then define "artificial no-go zones" on the map for it. I managed to achieve this by implementing a package which allows me to add custom PointCloud obstacles with different shapes to a custom layer on the costmaps.

The issue I am now facing is that when I am removing those obstacles from the map, they still stick to the local/global costmaps even though they have the "clearing" parameter set to True. This is problematic for me as I would like to have them disappear entirely once I am not publishing the corresponding PointCloud to the specific topic anymore.

What worked for me thus far is I have set a custom clearing behavior using the clear_costmap_recovery functionality from move_base, to only clear my virtual obstacles layer when move_base cannot plan a specific path due to a goal being posted inside one of these "ghost" virtual obstacles. When this happens the recovery behavior manages to clear the areas that remain in the costmaps.

What I would like to do now is to somehow call the clear_costmap_recovery behavior for my custom obstacle layer from inside my virtual_walls package whenever I delete an obstacle. I have been unable to find a tutorial or some piece of info that would point me to a good solution in this direction. For now I am calling the clear_costmaps service in its place but i am not sure if this is truly okay for my situation. I would just want to be able to clear that specific layer, programmatically, from inside my package.

Sorry for the long question, if any extra-info is needed please let me know and I will update my question! Hopefully there is some kind of solution for my issue!

Thanks!

EDIT 1: I would add also some RVIZ screenshots to further clarify this issue but for now I am unable to due to low karma points .

edit retag flag offensive close merge delete

add a comment

3 Answers

Sort by » oldest newest most voted

answered 2020-11-27 10:31:39 -0500

geoporus

51 ●2 ●5 ●12 https://www.linkedin.c...

Hello everyone!

Since I posted this question I have managed to somehow find a workaround for my problem. Unfortunately, after checking all the answers other people kindly posted to this thread I was unable to call the move_base clear_costmap_recovery behavior manually from a Python script and I kinda gave up trying after some time. If anyone else manages to achieve this and also stumbles upon this question, please post here how you did it, if you can!

To just bring a little context in, the bigger issue I was trying to address was trying to properly have some virtual obstacles (published as PointCloud points to a topic) marked as obstacles in local and global costmaps for move_base node and not only that but to properly clear them once that PointCloud is cleared or not published anymore. I couldn't achieve the deserved behavior by calling the clear_costmaps service or clear_costmap_recovery so what I did instead is, after I had my PointCloud properly formed and the coordinates for all the points properly computed, I created a separate static map layer in the form of an OccupancyGrid (with the same size as the map found in /map topic) to which I drew pixels according to the locations of the points in the PointCloud (had to do some simple translations from map origin but that was the biggest issue I encountered while attempting this). Now, after building this OccupancyGrid with the virtual obstacles drawn to it I just marked it as an observation source in costmap_common.yaml and then in costmap_global and costmap_local as type: "costmap_2d::StaticLayer".

After I did what I presented above, I managed to obtain the desired behavior for my virtual obstacles. They are now added and removed accordingly! If anyone else is interested in how I achieved this specifically, I can give some more information and also post some Rviz screenshots if needed! :)

edit flag offensive delete link

add a comment

answered 2020-11-16 11:24:15 -0500

tryan
1421 ●96 ●26

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.

edit flag offensive delete link

Comments

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 ...

(more)

geoporus ( 2020-11-17 04:47:58 -0500 )edit

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!

geoporus ( 2020-11-17 04:53:54 -0500 )edit

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.

tryan ( 2020-11-17 10:28:18 -0500 )edit

add a comment

answered 2020-11-16 10:40:22 -0500

JackB

977 ●10 ●76 ●33

I see in the move_base documentation that there is a clear_costmaps service. You can see the documentation of how to use services in Python here.

It may clear all costmaps, which my not be your intention exactly, but this seems like a good place to start as "services" in general are intended for the application type you are talking about here.

edit flag offensive delete link

Comments

Hey @JackB, thanks for your reply! As I have also stated in my question, I already use the clear_costmaps service as a temporary make-do solution for my problem. The main issue with this is that it indeed reverts all of the costmaps back to their initial, static state and this is not quite the behavior I am trying to get. I was hoping there would still be some way of calling the clear_costmap_behavior recovery which has this option of accepting layer_names as a parameter for a certain behavior. If I could call that then I could just clear my custom obstacles layer and my issue would be solved. This clear_costmap_behavior is implemented as a C++ ROS plugin so that's why I am not sure if I can call it from Python side.

geoporus ( 2020-11-17 03:04:03 -0500 )edit

Ah I should have read more closely! The python support of navstack is (as far as I understand) lacking unfortunately. If you look here I think you could find an avenue to add a layer specified clearing behavior.

JackB ( 2020-11-17 09:51:34 -0500 )edit

@JackB yeah, unfortunately it seems like, at least for ROS1, the python support of the whole navigation stack is not really there! Thanks for the last tip! I have already checked that code and it is fairly easy to understand how they set specific clearing recovery behaviors with specific layers using the parameter server. The issue still is that I cannot call the recovery behavior whenever I wish. I think I will look for different way of achieving the desired functionality. Thanks for all the help and your time!

geoporus ( 2020-11-18 02:51:30 -0500 )edit

add a comment

How to call move_base's clear_costmap_recovery functionality manually from Python

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to call move_base's clear_costmap_recovery functionality manually from Python edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to call move_base's clear_costmap_recovery functionality manually from Python