Go to the documentation of this file.00001 #include <vigir_footstep_planning_plugins/plugins/collision_check_grid_map_plugin.h>
00002
00003
00004
00005 namespace vigir_footstep_planning
00006 {
00007 CollisionCheckGridMapPlugin::CollisionCheckGridMapPlugin(const std::string& name)
00008 : CollisionCheckPlugin(name)
00009 , occ_thresh_(70)
00010 {
00011 }
00012
00013 bool CollisionCheckGridMapPlugin::initialize(const vigir_generic_params::ParameterSet& params)
00014 {
00015 if (!CollisionCheckPlugin::initialize(params))
00016 return false;
00017
00018 std::string topic;
00019 getParam("grid_map_topic", topic, std::string("/grid_map"));
00020 occupancy_grid_map_sub_ = nh_.subscribe<nav_msgs::OccupancyGrid>(topic, 1, &CollisionCheckGridMapPlugin::mapCallback, this);
00021
00022 getParam("occupancy_threshold", (int&) occ_thresh_, (int&) occ_thresh_, true);
00023
00024 return true;
00025 }
00026
00027 void CollisionCheckGridMapPlugin::reset()
00028 {
00029 CollisionCheckPlugin::reset();
00030
00031 boost::unique_lock<boost::shared_mutex> lock(grid_map_shared_mutex_);
00032 occupancy_grid_map_.reset();
00033 }
00034
00035 bool CollisionCheckGridMapPlugin::isCollisionCheckAvailable() const
00036 {
00037 return CollisionCheckPlugin::isCollisionCheckAvailable() && occupancy_grid_map_ != nullptr;
00038 }
00039
00040 bool CollisionCheckGridMapPlugin::isAccessible(const State& s) const
00041 {
00042 boost::shared_lock<boost::shared_mutex> lock(grid_map_shared_mutex_);
00043
00044 if (!occupancy_grid_map_)
00045 {
00046 ROS_ERROR_THROTTLE(10, "[CollisionCheckGridMapPlugin] No grid map available yet.");
00047 return true;
00048 }
00049
00050 double x = s.getX();
00051 double y = s.getY();
00052 int idx = 0;
00053
00054 if (getGridMapIndex(*occupancy_grid_map_, x, y, idx))
00055 return occupancy_grid_map_->data.at(idx) <= occ_thresh_;
00056
00057 return false;
00058 }
00059
00060 bool CollisionCheckGridMapPlugin::isAccessible(const State& next, const State& ) const
00061 {
00062 return isAccessible(next);
00063 }
00064
00065 void CollisionCheckGridMapPlugin::setOccupancyThreshold(unsigned char thresh)
00066 {
00067 occ_thresh_ = static_cast<int8_t>(thresh);
00068 }
00069
00070 void CollisionCheckGridMapPlugin::mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_map)
00071 {
00072 boost::unique_lock<boost::shared_mutex> lock(grid_map_shared_mutex_);
00073 this->occupancy_grid_map_ = occupancy_grid_map;
00074 }
00075 }
00076
00077 #include <pluginlib/class_list_macros.h>
00078 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::CollisionCheckGridMapPlugin, vigir_footstep_planning::CollisionCheckPlugin)