Ask Your Question

Check that a pose is free in a costmap

asked 2018-09-18 06:52:53 -0500

Hug gravatar image

updated 2018-09-18 08:17:06 -0500

Hi everyone,

I'm trying determine if a given (random) pose is free in a costmap.

My launch file looks like this:

  <node name="gp" pkg="global_planner" type="planner" output="screen">
    footprint: [[0.506, -0.32], [0.506, 0.32], [-0.454, 0.32], [-0.454, -0.32]]
      inflation_radius: 0.7
      - {name: 'static_layer', type: 'costmap_2d::StaticLayer'}
      - {name: inflater_layer, type: "costmap_2d::InflationLayer"}
  <node name="static_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /base_link 100"/>
  <node name="map_srv" pkg="map_server" type="map_server" args="$(find multirobot_sim)/maps/map_obstacles.yml"/>
  <node name="rviz_" pkg="rviz" type="rviz" args="-d $(find multirobot_sim)/rviz/config_multirobot.rviz"/>

I tried to use /gp/make_plan with the same pose for start and goal. But sometimes, a part of the footprint is stuck in an obstacle:

image description

I don't know if I made an error with the parameters or if using the planner for that purpose is simply not possible (for the inflation radius, I just put something slightly superior to the inscribed radius).

Did I miss something or do have a better idea?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2018-10-26 10:59:26 -0500

bmastay gravatar image

I've dealt with the same issue; I ended up using the occupancy_grid_utils library to calculate the cost of the cells inscribed in the footprint.

#include "occupancy_grid_utils/coordinate_conversions.h"
#include "occupancy_grid_utils/geometry.h"

namespace ogu = occupancy_grid_utils;
namespace gm = geometry_msgs;
namespace nm = nav_msgs;

bool isValidPosition(const gm::Polygon& oriented_footprint, const nm::OccupancyGrid& occupancy_grid,
                     const double grid_cost_cuttoff_p)
    auto cells = ogu::cellsInConvexPolygon(, oriented_footprint);
    for (auto& cell : cells)
      auto grid_idx = ogu::cellIndex(, cell);
      auto occupied_p =[grid_idx];
      if (occupied_p >= grid_cost_cuttoff_p)
        return false;
    return true;
  catch (ogu::CellOutOfBoundsException e)
    ROS_DEBUG("Cell out of bounds %s", e.what());
  catch (ogu::GridUtilsException e)
    ROS_ERROR("Error validating pose %s", e.what());
  return false;

Note I used


to get the oriented footprint

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2018-09-18 06:52:53 -0500

Seen: 349 times

Last updated: Oct 26 '18