Check that a pose is free in a costmap

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

Hug
66 ●8 ●9 ●16

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

add a comment

#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; }

Check that a pose is free in a costmap

1 Answer

Question Tools

Stats

Related questions

Check that a pose is free in a costmap edit

1 Answer

Question Tools

Stats

Related questions

Check that a pose is free in a costmap