sbpl_recovery.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2009, Willow Garage, Inc.
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 Willow Garage, Inc. 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  * Author: Eitan Marder-Eppstein
00036  *********************************************************************/
00037 
00038 #include <sbpl_recovery/sbpl_recovery.h>
00039 #include <pluginlib/class_list_macros.h>
00040 
00041 PLUGINLIB_DECLARE_CLASS(sbpl_recovery, SBPLRecovery, sbpl_recovery::SBPLRecovery,
00042     nav_core::RecoveryBehavior)
00043 
00044 namespace sbpl_recovery
00045 {
00046   SBPLRecovery::SBPLRecovery():
00047     global_costmap_(NULL),
00048     local_costmap_(NULL),
00049     tf_(NULL),
00050     initialized_(false)
00051   {
00052   }
00053 
00054   void SBPLRecovery::initialize (std::string n, tf::TransformListener* tf,
00055       costmap_2d::Costmap2DROS* global_costmap,
00056       costmap_2d::Costmap2DROS* local_costmap)
00057   {
00058     ros::NodeHandle nh = ros::NodeHandle();
00059     ros::NodeHandle p_nh = ros::NodeHandle("~/" + n);
00060 
00061     std::string plan_topic;
00062     p_nh.param("plan_topic", plan_topic, std::string("NavfnROS/plan"));
00063     p_nh.param("control_frequency", control_frequency_, 10.0);
00064     p_nh.param("controller_patience", controller_patience_, 5.0);
00065     p_nh.param("planning_attempts", planning_attempts_, 3);
00066     p_nh.param("attempts_per_run", attempts_per_run_, 3);
00067     p_nh.param("use_local_frame", use_local_frame_, true);
00068 
00069     double planning_distance;
00070     p_nh.param("planning_distance", planning_distance, 2.0);
00071     sq_planning_distance_ = planning_distance * planning_distance;
00072 
00073     //we need to initialize our costmaps
00074     global_costmap_ = global_costmap;
00075     local_costmap_ = local_costmap;
00076     tf_ = tf;
00077 
00078     //we need to initialize our local and global planners
00079     if(use_local_frame_)
00080       global_planner_.initialize(n + "/sbpl_lattice_planner", local_costmap_);
00081     else
00082       global_planner_.initialize(n + "/sbpl_lattice_planner", global_costmap_);
00083 
00084     local_planner_.initialize(n + "/pose_follower", tf, local_costmap_);
00085 
00086     //we'll need to subscribe to get the latest plan information 
00087     vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00088 
00089     ros::NodeHandle node_nh("~/");
00090     plan_sub_ = node_nh.subscribe<nav_msgs::Path>(plan_topic, 1, boost::bind(&SBPLRecovery::planCB, this, _1));
00091     initialized_ = true;
00092   }
00093 
00094   void SBPLRecovery::planCB(const nav_msgs::Path::ConstPtr& plan)
00095   {
00096     //just copy the plan data over
00097 
00098     tf::Stamped<tf::Pose> global_pose;
00099     !local_costmap_->getRobotPose(global_pose);
00100 
00101     costmap_2d::Costmap2D costmap;
00102     local_costmap_->getCostmapCopy(costmap);
00103 
00104     if(use_local_frame_)
00105     {
00106       std::vector<geometry_msgs::PoseStamped> transformed_plan;
00107       if(base_local_planner::transformGlobalPlan(*tf_, plan->poses, global_pose, costmap, local_costmap_->getGlobalFrameID(), transformed_plan))
00108       {
00109         boost::mutex::scoped_lock l(plan_mutex_);
00110         if(!transformed_plan.empty())
00111           plan_.header = transformed_plan[0].header;
00112         plan_.poses = transformed_plan;
00113       }
00114       else
00115         ROS_WARN("Could not transform to frame of the local recovery");
00116     }
00117     else
00118     {
00119       boost::mutex::scoped_lock l(plan_mutex_);
00120       plan_ = *plan;
00121     }
00122   }
00123 
00124   double SBPLRecovery::sqDistance(const geometry_msgs::PoseStamped& p1, 
00125       const geometry_msgs::PoseStamped& p2)
00126   {
00127     return (p1.pose.position.x - p2.pose.position.x) * (p1.pose.position.x - p2.pose.position.x) +
00128       (p1.pose.position.y - p2.pose.position.y) * (p1.pose.position.y - p2.pose.position.y);
00129   }
00130 
00131   std::vector<geometry_msgs::PoseStamped> SBPLRecovery::makePlan()
00132   {
00133     boost::mutex::scoped_lock l(plan_mutex_);
00134     std::vector<geometry_msgs::PoseStamped> sbpl_plan;
00135 
00136     tf::Stamped<tf::Pose> global_pose;
00137     if(use_local_frame_)
00138     {
00139       if(!local_costmap_->getRobotPose(global_pose))
00140       {
00141         ROS_ERROR("SBPL recovery behavior could not get the current pose of the robot. Doing nothing.");
00142         return sbpl_plan;
00143       }
00144     }
00145     else
00146     {
00147       if(!global_costmap_->getRobotPose(global_pose))
00148       {
00149         ROS_ERROR("SBPL recovery behavior could not get the current pose of the robot. Doing nothing.");
00150         return sbpl_plan;
00151       }
00152     }
00153 
00154     geometry_msgs::PoseStamped start;
00155     tf::poseStampedTFToMsg(global_pose, start);
00156 
00157     //first, we want to walk far enough along the path that we get to a point
00158     //that is within the recovery distance from the robot. Otherwise, we might
00159     //move backwards along the plan
00160     unsigned int index = 0;
00161     for(index=0; index < plan_.poses.size(); ++index)
00162     {
00163       if(sqDistance(start, plan_.poses[index]) < sq_planning_distance_)
00164         break;
00165     }
00166 
00167     //next, we want to find a goal point that is far enough away from the robot on the
00168     //original plan and attempt to plan to it
00169     int unsuccessful_attempts = 0;
00170     for(unsigned int i = index; i < plan_.poses.size(); ++i)
00171     {
00172       ROS_DEBUG("SQ Distance: %.2f,  spd: %.2f, start (%.2f, %.2f), goal (%.2f, %.2f)",
00173           sqDistance(start, plan_.poses[i]),
00174           sq_planning_distance_,
00175           start.pose.position.x, start.pose.position.y,
00176           plan_.poses[i].pose.position.x,
00177           plan_.poses[i].pose.position.y);
00178       if(sqDistance(start, plan_.poses[i]) >= sq_planning_distance_ || i == (plan_.poses.size() - 1))
00179       {
00180         ROS_INFO("Calling sbpl planner with start (%.2f, %.2f), goal (%.2f, %.2f)",
00181             start.pose.position.x, start.pose.position.y,
00182             plan_.poses[i].pose.position.x,
00183             plan_.poses[i].pose.position.y);
00184         if(global_planner_.makePlan(start, plan_.poses[i], sbpl_plan) && !sbpl_plan.empty())
00185         {
00186           ROS_INFO("Got a valid plan");
00187           return sbpl_plan;
00188         }
00189         sbpl_plan.clear();
00190 
00191         //make sure that we don't spend forever planning
00192         unsuccessful_attempts++;
00193         if(unsuccessful_attempts >= attempts_per_run_)
00194           return sbpl_plan;
00195       }
00196     }
00197 
00198     return sbpl_plan;
00199   }
00200 
00201   void SBPLRecovery::runBehavior()
00202   {
00203     if(!initialized_)
00204     {
00205       ROS_ERROR("Please initialize this recovery behavior before attempting to run it.");
00206       return;
00207     }
00208 
00209     ROS_INFO("Starting the sbpl recovery behavior");
00210 
00211     for(int i=0; i <= planning_attempts_; ++i)
00212     {
00213       std::vector<geometry_msgs::PoseStamped> sbpl_plan = makePlan();
00214 
00215       if(sbpl_plan.empty())
00216       {
00217         ROS_ERROR("Unable to find a valid pose to plan to on the global plan.");
00218         return;
00219       }
00220 
00221       //if we've got a valid plan, we're going to clear unknown space out to our planning distance
00222       //in our costmaps, not the safest thing to do, but we're trying to get out
00223       double window_size = 2 * sqrt(sq_planning_distance_);
00224       global_costmap_->clearNonLethalWindow(window_size, window_size);
00225       local_costmap_->clearNonLethalWindow(window_size, window_size);
00226 
00227       //ok... now we've got a plan so we need to try to follow it
00228       local_planner_.setPlan(sbpl_plan);
00229       ros::Rate r(control_frequency_);
00230       ros::Time last_valid_control = ros::Time::now();
00231       while(ros::ok() && 
00232           last_valid_control + ros::Duration(controller_patience_) >= ros::Time::now() && 
00233           !local_planner_.isGoalReached())
00234       {
00235         geometry_msgs::Twist cmd_vel;
00236         bool valid_control = local_planner_.computeVelocityCommands(cmd_vel);
00237 
00238         if(valid_control)
00239           last_valid_control = ros::Time::now();
00240 
00241         vel_pub_.publish(cmd_vel);
00242         r.sleep();
00243       }
00244 
00245       if(local_planner_.isGoalReached())
00246         ROS_INFO("The sbpl recovery behavior made it to its desired goal");
00247       else
00248         ROS_WARN("The sbpl recovery behavior failed to make it to its desired goal");
00249     }
00250   }
00251 };


sbpl_recovery
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Oct 6 2014 02:49:26