explore.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include <explore/explore.h>
00038 #include <explore/explore_frontier.h>
00039 
00040 #include <boost/thread.hpp>
00041 #include <boost/bind.hpp>
00042 
00043 using namespace costmap_2d;
00044 using namespace navfn;
00045 using namespace visualization_msgs;
00046 using namespace geometry_msgs;
00047 
00048 namespace explore {
00049 
00050 double sign(double x){
00051   return x < 0.0 ? -1.0 : 1.0;
00052 }
00053 
00054 Explore::Explore() :
00055   node_(),
00056   tf_(ros::Duration(10.0)),
00057   explore_costmap_ros_(NULL),
00058   move_base_client_("move_base"),
00059   planner_(NULL),
00060   done_exploring_(false),
00061   explorer_(NULL),
00062   prev_plan_size_(0)
00063 {
00064   ros::NodeHandle private_nh("~");
00065 
00066   marker_publisher_ = node_.advertise<Marker>("visualization_marker",10);
00067   marker_array_publisher_ = node_.advertise<MarkerArray>("visualization_marker_array",10);
00068   map_publisher_ = private_nh.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00069   map_server_ = private_nh.advertiseService("explore_map", &Explore::mapCallback, this);
00070 
00071   private_nh.param("navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
00072   private_nh.param("planner_frequency", planner_frequency_, 1.0);
00073   private_nh.param("progress_timeout", progress_timeout_, 30.0);
00074   private_nh.param("visualize", visualize_, 1);
00075   double loop_closure_addition_dist_min;
00076   double loop_closure_loop_dist_min;
00077   double loop_closure_loop_dist_max;
00078   double loop_closure_slam_entropy_max;
00079   private_nh.param("close_loops", close_loops_, false); // TODO: switch default to true once gmapping 1.1 has been released
00080   private_nh.param("loop_closure_addition_dist_min", loop_closure_addition_dist_min, 2.5);
00081   private_nh.param("loop_closure_loop_dist_min", loop_closure_loop_dist_min, 6.0);
00082   private_nh.param("loop_closure_loop_dist_max", loop_closure_loop_dist_max, 20.0);
00083   private_nh.param("loop_closure_slam_entropy_max", loop_closure_slam_entropy_max, 3.0);
00084   private_nh.param("potential_scale", potential_scale_, 1e-3);
00085   private_nh.param("orientation_scale", orientation_scale_, 0.0); // TODO: set this back to 0.318 once getOrientationChange is fixed
00086   private_nh.param("gain_scale", gain_scale_, 1.0);
00087 
00088   explore_costmap_ros_ = new Costmap2DROS(std::string("explore_costmap"), tf_);
00089   explore_costmap_ros_->clearRobotFootprint();
00090 
00091   planner_ = new navfn::NavfnROS(std::string("explore_planner"), explore_costmap_ros_);
00092   explorer_ = new ExploreFrontier();
00093   loop_closure_ = new LoopClosure(loop_closure_addition_dist_min,
00094         loop_closure_loop_dist_min,
00095         loop_closure_loop_dist_max,
00096         loop_closure_slam_entropy_max,
00097         planner_frequency_,
00098         move_base_client_,
00099         *explore_costmap_ros_,
00100         client_mutex_);
00101 }
00102 
00103 Explore::~Explore() {
00104   if(loop_closure_ != NULL)
00105     delete loop_closure_;
00106 
00107   if(planner_ != NULL)
00108     delete planner_;
00109 
00110   if(explorer_ != NULL)
00111     delete explorer_;
00112 
00113   if(explore_costmap_ros_ != NULL)
00114     delete explore_costmap_ros_;
00115 }
00116 
00117 bool Explore::mapCallback(nav_msgs::GetMap::Request  &req,
00118                           nav_msgs::GetMap::Response &res)
00119 {
00120   ROS_DEBUG("mapCallback");
00121   Costmap2D explore_costmap;
00122   explore_costmap_ros_->getCostmapCopy(explore_costmap);
00123 
00124   res.map.info.width = explore_costmap.getSizeInCellsX();
00125   res.map.info.height = explore_costmap.getSizeInCellsY();
00126   res.map.info.resolution = explore_costmap.getResolution();
00127   res.map.info.origin.position.x = explore_costmap.getOriginX();
00128   res.map.info.origin.position.y = explore_costmap.getOriginY();
00129   res.map.info.origin.position.z = 0;
00130   res.map.info.origin.orientation.x = 0;
00131   res.map.info.origin.orientation.y = 0;
00132   res.map.info.origin.orientation.z = 0;
00133   res.map.info.origin.orientation.w = 1;
00134 
00135   int size = res.map.info.width * res.map.info.height;
00136   const unsigned char* map = explore_costmap.getCharMap();
00137 
00138   res.map.data.resize((size_t)size);
00139   for (int i=0; i<size; i++) {
00140     if (map[i] == NO_INFORMATION)
00141       res.map.data[i] = -1;
00142     else if (map[i] == LETHAL_OBSTACLE)
00143       res.map.data[i] = 100;
00144     else
00145       res.map.data[i] = 0;
00146   }
00147 
00148   return true;
00149 }
00150 
00151 void Explore::publishMap() {
00152   nav_msgs::OccupancyGrid map;
00153   map.header.stamp = ros::Time::now();
00154 
00155   Costmap2D explore_costmap;
00156   explore_costmap_ros_->getCostmapCopy(explore_costmap);
00157 
00158   map.info.width = explore_costmap.getSizeInCellsX();
00159   map.info.height = explore_costmap.getSizeInCellsY();
00160   map.info.resolution = explore_costmap.getResolution();
00161   map.info.origin.position.x = explore_costmap.getOriginX();
00162   map.info.origin.position.y = explore_costmap.getOriginY();
00163   map.info.origin.position.z = 0;
00164   map.info.origin.orientation.x = 0;
00165   map.info.origin.orientation.y = 0;
00166   map.info.origin.orientation.z = 0;
00167   map.info.origin.orientation.w = 1;
00168 
00169   int size = map.info.width * map.info.height;
00170   const unsigned char* char_map = explore_costmap.getCharMap();
00171 
00172   map.data.resize((size_t)size);
00173   for (int i=0; i<size; i++) {
00174     if (char_map[i] == NO_INFORMATION)
00175       map.data[i] = -1;
00176     else if (char_map[i] == LETHAL_OBSTACLE)
00177       map.data[i] = 100;
00178     else
00179       map.data[i] = 0;
00180   }
00181 
00182   map_publisher_.publish(map);
00183 }
00184 
00185 void Explore::publishGoal(const geometry_msgs::Pose& goal){
00186   visualization_msgs::Marker marker;
00187   marker.header.frame_id = "map";
00188   marker.header.stamp = ros::Time::now();
00189   marker.ns = "explore_goal";
00190   marker.id = 0;
00191   marker.type = visualization_msgs::Marker::ARROW;
00192   marker.pose = goal;
00193   marker.scale.x = 0.5;
00194   marker.scale.y = 1.0;
00195   marker.scale.z = 1.0;
00196   marker.color.a = 1.0;
00197   marker.color.r = 0.0;
00198   marker.color.g = 1.0;
00199   marker.color.b = 0.0;
00200   marker.lifetime = ros::Duration(5);
00201   marker_publisher_.publish(marker);
00202 }
00203 
00204 void Explore::makePlan() {
00205   //since this gets called on handle activate
00206   if(explore_costmap_ros_ == NULL)
00207     return;
00208 
00209   tf::Stamped<tf::Pose> robot_pose;
00210   explore_costmap_ros_->getRobotPose(robot_pose);
00211 
00212   std::vector<geometry_msgs::Pose> goals;
00213   explore_costmap_ros_->clearRobotFootprint();
00214   explorer_->getExplorationGoals(*explore_costmap_ros_, robot_pose, planner_, goals, potential_scale_, orientation_scale_, gain_scale_);
00215   if (goals.size() == 0)
00216     done_exploring_ = true;
00217 
00218   bool valid_plan = false;
00219   std::vector<geometry_msgs::PoseStamped> plan;
00220   PoseStamped goal_pose, robot_pose_msg;
00221   tf::poseStampedTFToMsg(robot_pose, robot_pose_msg);
00222 
00223   goal_pose.header.frame_id = explore_costmap_ros_->getGlobalFrameID();
00224   goal_pose.header.stamp = ros::Time::now();
00225   planner_->computePotential(robot_pose_msg.pose.position); // just to be safe, though this should already have been done in explorer_->getExplorationGoals
00226   int blacklist_count = 0;
00227   for (unsigned int i=0; i<goals.size(); i++) {
00228     goal_pose.pose = goals[i];
00229     if (goalOnBlacklist(goal_pose)) {
00230       blacklist_count++;
00231       continue;
00232     }
00233 
00234     valid_plan = ((planner_->getPlanFromPotential(goal_pose, plan)) && (!plan.empty()));
00235     if (valid_plan) {
00236       break;
00237     }
00238   }
00239 
00240   // publish visualization markers
00241   if (visualize_) {
00242     std::vector<Marker> markers;
00243     explorer_->getVisualizationMarkers(markers);
00244     for (uint i=0; i < markers.size(); i++)
00245       marker_publisher_.publish(markers[i]);
00246   }
00247 
00248   if (valid_plan) {
00249     if (prev_plan_size_ != plan.size()) {
00250       time_since_progress_ = 0.0;
00251     } else {
00252       double dx = prev_goal_.pose.position.x - goal_pose.pose.position.x;
00253       double dy = prev_goal_.pose.position.y - goal_pose.pose.position.y;
00254       double dist = sqrt(dx*dx+dy*dy);
00255       if (dist < 0.01) {
00256         time_since_progress_ += 1.0f / planner_frequency_;
00257       }
00258     }
00259 
00260     // black list goals for which we've made no progress for a long time
00261     if (time_since_progress_ > progress_timeout_) {
00262       frontier_blacklist_.push_back(goal_pose);
00263       ROS_DEBUG("Adding current goal to black list");
00264     }
00265 
00266     prev_plan_size_ = plan.size();
00267     prev_goal_ = goal_pose;
00268 
00269     move_base_msgs::MoveBaseGoal goal;
00270     goal.target_pose = goal_pose;
00271     move_base_client_.sendGoal(goal, boost::bind(&Explore::reachedGoal, this, _1, _2, goal_pose));
00272 
00273     if (visualize_) {
00274       publishGoal(goal_pose.pose);
00275       publishMap();
00276     }
00277   } else {
00278     ROS_WARN("Done exploring with %d goals left that could not be reached. There are %d goals on our blacklist, and %d of the frontier goals are too close to them to pursue. The rest had global planning fail to them. \n", (int)goals.size(), (int)frontier_blacklist_.size(), blacklist_count);
00279     ROS_INFO("Exploration finished. Hooray.");
00280     done_exploring_ = true;
00281   }
00282 
00283 }
00284 
00285 bool Explore::goalOnBlacklist(const geometry_msgs::PoseStamped& goal){
00286   //check if a goal is on the blacklist for goals that we're pursuing
00287   for(unsigned int i = 0; i < frontier_blacklist_.size(); ++i){
00288     double x_diff = fabs(goal.pose.position.x - frontier_blacklist_[i].pose.position.x);
00289     double y_diff = fabs(goal.pose.position.y - frontier_blacklist_[i].pose.position.y);
00290 
00291     if(x_diff < 2 * explore_costmap_ros_->getResolution() && y_diff < 2 * explore_costmap_ros_->getResolution())
00292       return true;
00293   }
00294   return false;
00295 }
00296 
00297 void Explore::reachedGoal(const actionlib::SimpleClientGoalState& status,
00298     const move_base_msgs::MoveBaseResultConstPtr& result, geometry_msgs::PoseStamped frontier_goal){
00299 
00300   ROS_DEBUG("Reached goal");
00301   if(status == actionlib::SimpleClientGoalState::ABORTED){
00302     frontier_blacklist_.push_back(frontier_goal);
00303     ROS_DEBUG("Adding current goal to black list");
00304   }
00305 
00306 //  if(!done_exploring_){
00307 //    //create a plan from the frontiers left and send a new goal to move_base
00308 //    makePlan();
00309 //  }
00310 //  else{
00311 //    ROS_INFO("Exploration finished. Hooray.");
00312 //  }
00313 }
00314 
00315 void Explore::execute() {
00316   while (! move_base_client_.waitForServer(ros::Duration(5,0)))
00317     ROS_WARN("Waiting to connect to move_base server");
00318 
00319   ROS_INFO("Connected to move_base server");
00320 
00321   // This call sends the first goal, and sets up for future callbacks.
00322   makePlan();
00323 
00324   ros::Rate r(planner_frequency_);
00325   while (node_.ok() && (!done_exploring_)) {
00326 
00327     if (close_loops_) {
00328       tf::Stamped<tf::Pose> robot_pose;
00329       explore_costmap_ros_->getRobotPose(robot_pose);
00330       loop_closure_->updateGraph(robot_pose);
00331     }
00332 
00333     makePlan();
00334     r.sleep();
00335   }
00336 
00337   move_base_client_.cancelAllGoals();
00338 }
00339 
00340 void Explore::spin() {
00341   ros::spinOnce();
00342   boost::thread t(boost::bind( &Explore::execute, this ));
00343   ros::spin();
00344   t.join();
00345 }
00346 
00347 }
00348 
00349 int main(int argc, char** argv){
00350   ros::init(argc, argv, "explore");
00351 
00352   explore::Explore explore;
00353   explore.spin();
00354 
00355   return(0);
00356 }
00357 


explore
Author(s): Charles DuHadway (maintained by Benjamin Pitzer)
autogenerated on Sun Oct 5 2014 23:56:48