bounded_explore_layer.cpp
Go to the documentation of this file.
00001 #include <frontier_exploration/bounded_explore_layer.h>
00002 
00003 #include <pluginlib/class_list_macros.h>
00004 #include <geometry_msgs/PolygonStamped.h>
00005 #include <costmap_2d/costmap_2d.h>
00006 #include <costmap_2d/footprint.h>
00007 #include <sensor_msgs/PointCloud2.h>
00008 #include <pcl_ros/point_cloud.h>
00009 #include <pcl/point_cloud.h>
00010 #include <pcl/point_types.h>
00011 #include <boost/foreach.hpp>
00012 
00013 #include <frontier_exploration/Frontier.h>
00014 #include <frontier_exploration/UpdateBoundaryPolygon.h>
00015 #include <frontier_exploration/GetNextFrontier.h>
00016 #include <frontier_exploration/frontier_search.h>
00017 #include <frontier_exploration/geometry_tools.h>
00018 
00019 PLUGINLIB_EXPORT_CLASS(frontier_exploration::BoundedExploreLayer, costmap_2d::Layer)
00020 
00021 namespace frontier_exploration
00022 {
00023 
00024     using costmap_2d::LETHAL_OBSTACLE;
00025     using costmap_2d::NO_INFORMATION;
00026     using costmap_2d::FREE_SPACE;
00027 
00028     BoundedExploreLayer::BoundedExploreLayer(){}
00029 
00030     BoundedExploreLayer::~BoundedExploreLayer(){
00031         polygonService_.shutdown();
00032         frontierService_.shutdown();
00033         delete dsrv_;
00034         dsrv_ = 0;
00035     }
00036 
00037     void BoundedExploreLayer::onInitialize(){
00038 
00039         ros::NodeHandle nh_("~/" + name_);
00040         frontier_cloud_pub = nh_.advertise<sensor_msgs::PointCloud2>("frontiers",5);
00041         configured_ = false;
00042         marked_ = false;
00043 
00044         bool explore_clear_space;
00045         nh_.param("explore_clear_space", explore_clear_space, true);
00046         if(explore_clear_space){
00047             default_value_ = NO_INFORMATION;
00048         }else{
00049             default_value_ = FREE_SPACE;
00050         }
00051 
00052         matchSize();
00053 
00054         nh_.param<bool>("resize_to_boundary", resize_to_boundary_, true);
00055         nh_.param<std::string>("frontier_travel_point", frontier_travel_point_, "closest");
00056 
00057         polygonService_ = nh_.advertiseService("update_boundary_polygon", &BoundedExploreLayer::updateBoundaryPolygonService, this);
00058         frontierService_ = nh_.advertiseService("get_next_frontier", &BoundedExploreLayer::getNextFrontierService, this);
00059 
00060         dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh_);
00061         dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
00062                     &BoundedExploreLayer::reconfigureCB, this, _1, _2);
00063         dsrv_->setCallback(cb);
00064 
00065     }
00066 
00067 
00068     void BoundedExploreLayer::matchSize(){
00069         Costmap2D* master = layered_costmap_->getCostmap();
00070         resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
00071                   master->getOriginX(), master->getOriginY());
00072     }
00073 
00074 
00075     void BoundedExploreLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level){
00076         enabled_ = config.enabled;
00077     }
00078 
00079     bool BoundedExploreLayer::getNextFrontierService(frontier_exploration::GetNextFrontier::Request &req, frontier_exploration::GetNextFrontier::Response &res){
00080         return getNextFrontier(req.start_pose, res.next_frontier);
00081     }
00082 
00083     bool BoundedExploreLayer::getNextFrontier(geometry_msgs::PoseStamped start_pose, geometry_msgs::PoseStamped &next_frontier){
00084 
00085         //wait for costmap to get marked with boundary
00086         ros::Rate r(10);
00087         while(!marked_){
00088             ros::spinOnce();
00089             r.sleep();
00090         }
00091 
00092         if(start_pose.header.frame_id != layered_costmap_->getGlobalFrameID()){
00093             //error out if no transform available
00094             if(!tf_listener_.waitForTransform(layered_costmap_->getGlobalFrameID(), start_pose.header.frame_id,ros::Time::now(),ros::Duration(10))) {
00095                 ROS_ERROR_STREAM("Couldn't transform from "<<layered_costmap_->getGlobalFrameID()<<" to "<< start_pose.header.frame_id);
00096                 return false;
00097             }
00098             geometry_msgs::PoseStamped temp_pose = start_pose;
00099             tf_listener_.transformPose(layered_costmap_->getGlobalFrameID(),temp_pose,start_pose);
00100         }
00101 
00102         //initialize frontier search implementation
00103         FrontierSearch frontierSearch(*(layered_costmap_->getCostmap()));
00104         //get list of frontiers from search implementation
00105         std::list<Frontier> frontier_list = frontierSearch.searchFrom(start_pose.pose.position);
00106 
00107         if(frontier_list.size() == 0){
00108             ROS_DEBUG("No frontiers found, exploration complete");
00109             return false;
00110         }
00111 
00112         //create placeholder for selected frontier
00113         Frontier selected;
00114         selected.min_distance = std::numeric_limits<double>::infinity();
00115 
00116         //pointcloud for visualization purposes
00117         pcl::PointCloud<pcl::PointXYZI> frontier_cloud_viz;
00118         pcl::PointXYZI frontier_point_viz(50);
00119         int max;
00120 
00121         BOOST_FOREACH(Frontier frontier, frontier_list){
00122             //load frontier into visualization poitncloud
00123             frontier_point_viz.x = frontier.initial.x;
00124             frontier_point_viz.y = frontier.initial.y;
00125             frontier_cloud_viz.push_back(frontier_point_viz);
00126 
00127             //check if this frontier is the nearest to robot
00128             if (frontier.min_distance < selected.min_distance){
00129                 selected = frontier;
00130                 max = frontier_cloud_viz.size()-1;
00131             }
00132         }
00133 
00134         //color selected frontier
00135         frontier_cloud_viz[max].intensity = 100;
00136 
00137         //publish visualization point cloud
00138         sensor_msgs::PointCloud2 frontier_viz_output;
00139         pcl::toROSMsg(frontier_cloud_viz,frontier_viz_output);
00140         frontier_viz_output.header.frame_id = layered_costmap_->getGlobalFrameID();
00141         frontier_viz_output.header.stamp = ros::Time::now();
00142         frontier_cloud_pub.publish(frontier_viz_output);
00143 
00144         //set goal pose to next frontier
00145         next_frontier.header.frame_id = layered_costmap_->getGlobalFrameID();
00146         next_frontier.header.stamp = ros::Time::now();
00147 
00148         //
00149         if(frontier_travel_point_ == "closest"){
00150             next_frontier.pose.position = selected.initial;
00151         }else if(frontier_travel_point_ == "middle"){
00152             next_frontier.pose.position = selected.middle;
00153         }else if(frontier_travel_point_ == "centroid"){
00154             next_frontier.pose.position = selected.centroid;
00155         }else{
00156             ROS_ERROR("Invalid 'frontier_travel_point' parameter, falling back to 'closest'");
00157             next_frontier.pose.position = selected.initial;
00158         }
00159 
00160         next_frontier.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(start_pose.pose.position, next_frontier.pose.position) );
00161         return true;
00162 
00163     }
00164 
00165     bool BoundedExploreLayer::updateBoundaryPolygonService(frontier_exploration::UpdateBoundaryPolygon::Request &req, frontier_exploration::UpdateBoundaryPolygon::Response &res){
00166 
00167         return updateBoundaryPolygon(req.explore_boundary);
00168 
00169     }
00170 
00171     void BoundedExploreLayer::reset(){
00172 
00173         //reset costmap_ char array to default values
00174         marked_ = false;
00175         configured_ = false;
00176         memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
00177 
00178     }
00179 
00180     bool BoundedExploreLayer::updateBoundaryPolygon(geometry_msgs::PolygonStamped polygon_stamped){
00181 
00182         //clear existing boundary, if any
00183         polygon_.points.clear();
00184 
00185         //error if no transform available between polygon and costmap
00186         if(!tf_listener_.waitForTransform(layered_costmap_->getGlobalFrameID(), polygon_stamped.header.frame_id,ros::Time::now(),ros::Duration(10))) {
00187             ROS_ERROR_STREAM("Couldn't transform from "<<layered_costmap_->getGlobalFrameID()<<" to "<< polygon_stamped.header.frame_id);
00188             return false;
00189         }
00190 
00191         //Transform all points of boundary polygon into costmap frame
00192         geometry_msgs::PointStamped in, out;
00193         in.header = polygon_stamped.header;
00194         BOOST_FOREACH(geometry_msgs::Point32 point32, polygon_stamped.polygon.points){
00195             in.point = costmap_2d::toPoint(point32);
00196             tf_listener_.transformPoint(layered_costmap_->getGlobalFrameID(),in,out);
00197             polygon_.points.push_back(costmap_2d::toPoint32(out.point));
00198         }
00199 
00200         //if empty boundary provided, set to whole map
00201         if(polygon_.points.empty()){
00202             geometry_msgs::Point32 temp;
00203             temp.x = getOriginX();
00204             temp.y = getOriginY();
00205             polygon_.points.push_back(temp);
00206             temp.y = getSizeInMetersY();
00207             polygon_.points.push_back(temp);
00208             temp.x = getSizeInMetersX();
00209             polygon_.points.push_back(temp);
00210             temp.y = getOriginY();
00211             polygon_.points.push_back(temp);
00212         }
00213 
00214         if(resize_to_boundary_){
00215             updateOrigin(0,0);
00216 
00217             //Find map size and origin by finding min/max points of polygon
00218             double min_x = std::numeric_limits<double>::infinity();
00219             double min_y = std::numeric_limits<double>::infinity();
00220             double max_x = -std::numeric_limits<double>::infinity();
00221             double max_y = -std::numeric_limits<double>::infinity();
00222 
00223             BOOST_FOREACH(geometry_msgs::Point32 point, polygon_.points){
00224                 min_x = std::min(min_x,(double)point.x);
00225                 min_y = std::min(min_y,(double)point.y);
00226                 max_x = std::max(max_x,(double)point.x);
00227                 max_y = std::max(max_y,(double)point.y);
00228             }
00229 
00230             //resize the costmap to polygon boundaries, don't change resolution
00231             int size_x, size_y;
00232             worldToMapNoBounds(max_x - min_x, max_y - min_y, size_x, size_y);
00233             layered_costmap_->resizeMap(size_x, size_y, layered_costmap_->getCostmap()->getResolution(), min_x, min_y);
00234             matchSize();
00235         }
00236 
00237         configured_ = true;
00238         marked_ = false;
00239         return true;
00240     }
00241 
00242 
00243     void BoundedExploreLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00244                                            double* min_y, double* max_x, double* max_y){
00245 
00246         //check if layer is enabled and configured with a boundary
00247         if (!enabled_ || !configured_){ return; }
00248 
00249         //update the whole costmap
00250         *min_x = getOriginX();
00251         *min_y = getOriginY();
00252         *max_x = getSizeInMetersX()+getOriginX();
00253         *max_y = getSizeInMetersY()+getOriginY();
00254 
00255     }
00256 
00257     void BoundedExploreLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
00258         //check if layer is enabled and configured with a boundary
00259         if (!enabled_ || !configured_){ return; }
00260 
00261         //draw lines between each point in polygon
00262         MarkCell marker(costmap_, LETHAL_OBSTACLE);
00263 
00264         //circular iterator
00265         for(int i = 0, j = polygon_.points.size()-1; i < polygon_.points.size(); j = i++){
00266 
00267             int x_1, y_1, x_2, y_2;
00268             worldToMapEnforceBounds(polygon_.points[i].x, polygon_.points[i].y, x_1,y_1);
00269             worldToMapEnforceBounds(polygon_.points[j].x, polygon_.points[j].y, x_2,y_2);
00270 
00271             raytraceLine(marker,x_1,y_1,x_2,y_2);
00272         }
00273         //update the master grid from the internal costmap
00274         mapUpdateKeepObstacles(master_grid, min_i, min_j, max_i, max_j);
00275 
00276 
00277     }
00278 
00279     void BoundedExploreLayer::mapUpdateKeepObstacles(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
00280         if (!enabled_)
00281             return;
00282 
00283         unsigned char* master = master_grid.getCharMap();
00284         unsigned int span = master_grid.getSizeInCellsX();
00285 
00286         for (int j = min_j; j < max_j; j++)
00287         {
00288             unsigned int it = span*j+min_i;
00289             for (int i = min_i; i < max_i; i++)
00290             {
00291                 //only update master grid if local costmap cell is lethal/higher value, and is not overwriting a lethal obstacle in the master grid
00292                 if(master[it] != LETHAL_OBSTACLE && (costmap_[it] == LETHAL_OBSTACLE || costmap_[it] > master[it])){
00293                     master[it] = costmap_[it];
00294                 }
00295                 it++;
00296             }
00297         }
00298         marked_ = true;
00299     }
00300 }


frontier_exploration
Author(s): Paul Bovbel
autogenerated on Thu Jun 6 2019 20:45:41