Ask Your Question
0

Check that a pose is free in a costmap

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

Hug gravatar image

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

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:

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
0

answered 2018-10-26 10:59:26 -0600

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)
{
  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;
}

Note I used

costmap2d::transformFootprint()

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

2 followers

Stats

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

Seen: 145 times

Last updated: Oct 26 '18