Hi everyone,

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

My launch file looks like this:

<launch>
  <node name="gp" pkg="global_planner" type="planner" output="screen">
  <rosparam>
  costmap:
    footprint: [[0.506, -0.32], [0.506, 0.32], [-0.454, 0.32], [-0.454, -0.32]]
    inflater_layer:
      inflation_radius: 0.7
    plugins:
      - {name: 'static_layer', type: 'costmap_2d::StaticLayer'}
      - {name: inflater_layer, type: "costmap_2d::InflationLayer"}
  </rosparam>
  </node>
  <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"/>
</launch>

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:

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?

#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)
{
  try
  {
    auto cells = ogu::cellsInConvexPolygon(occupancy_grid.info, oriented_footprint);
    for (auto& cell : cells)
    {
      auto grid_idx = ogu::cellIndex(occupancy_grid.info, cell);
      auto occupied_p = occupancy_grid.data[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;
}

costmap2d::transformFootprint()