eband_local_planner_ros.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, 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: Christian Connette
00036 *********************************************************************/
00037 
00038 #include <eband_local_planner/eband_local_planner_ros.h>
00039 
00040 // pluginlib macros (defines, ...)
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 // abstract class from which our plugin inherits
00044 #include <nav_core/base_local_planner.h>
00045 
00046 
00047 // register this planner as a BaseGlobalPlanner plugin
00048 // (see http://www.ros.org/wiki/pluginlib/Tutorials/Writing%20and%20Using%20a%20Simple%20Plugin)
00049 PLUGINLIB_DECLARE_CLASS(eband_local_planner, EBandPlannerROS, eband_local_planner::EBandPlannerROS, nav_core::BaseLocalPlanner)
00050 
00051 
00052 namespace eband_local_planner{
00053 
00054 EBandPlannerROS::EBandPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}
00055 
00056 
00057 EBandPlannerROS::EBandPlannerROS(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
00058  : costmap_ros_(NULL), tf_(NULL), initialized_(false)
00059 {
00060         // initialize planner
00061         initialize(name, tf, costmap_ros);
00062 }
00063 
00064 
00065 EBandPlannerROS::~EBandPlannerROS() {}
00066 
00067 
00068 void EBandPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
00069 {
00070         // check if the plugin is already initialized
00071         if(!initialized_)
00072         {
00073                 // copy adress of costmap and Transform Listener (handed over from move_base)
00074                 costmap_ros_ = costmap_ros;
00075                 tf_ = tf;
00076 
00077 
00078                 // create Node Handle with name of plugin (as used in move_base for loading)
00079                 ros::NodeHandle pn("~/" + name);
00080 
00081                 // read parameters from parameter server
00082                 // get tolerances for "Target reached"
00083                 pn.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
00084                 pn.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1);
00085 
00086                 // set lower bound for velocity -> if velocity in this region stop! (to avoid limit-cycles or lock)
00087                 pn.param("rot_stopped_vel", rot_stopped_vel_, 1e-2);
00088                 pn.param("trans_stopped_vel", trans_stopped_vel_, 1e-2);
00089 
00090                 // advertise topics (adapted global plan and predicted local trajectory)
00091                 g_plan_pub_ = pn.advertise<nav_msgs::Path>("global_plan", 1);
00092                 l_plan_pub_ = pn.advertise<nav_msgs::Path>("local_plan", 1);
00093 
00094 
00095                 // subscribe to topics (to get odometry information, we need to get a handle to the topic in the global namespace)
00096                 ros::NodeHandle gn;
00097                 odom_sub_ = gn.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&EBandPlannerROS::odomCallback, this, _1));
00098 
00099 
00100                 // create the actual planner that we'll use. Pass Name of plugin and pointer to global costmap to it.
00101                 // (configuration is done via parameter server)
00102                 eband_ = boost::shared_ptr<EBandPlanner>(new EBandPlanner(name, costmap_ros_));
00103 
00104                 // create the according controller
00105                 eband_trj_ctrl_ = boost::shared_ptr<EBandTrajectoryCtrl>(new EBandTrajectoryCtrl(name, costmap_ros_));
00106 
00107                 // create object for visualization
00108                 eband_visual_ = boost::shared_ptr<EBandVisualization>(new EBandVisualization);
00109 
00110                 // pass visualization object to elastic band
00111                 eband_->setVisualization(eband_visual_);
00112 
00113                 // pass visualization object to controller
00114                 eband_trj_ctrl_->setVisualization(eband_visual_);
00115 
00116                 // initialize visualization - set node handle and pointer to costmap
00117                 eband_visual_->initialize(pn, costmap_ros);
00118 
00119 
00120                 // set initialized flag
00121                 initialized_ = true;
00122 
00123 
00124                 // this is only here to make this process visible in the rxlogger right from the start
00125                 ROS_DEBUG("Elastic Band plugin initialized.");
00126         }
00127         else
00128         {
00129                 ROS_WARN("This planner has already been initialized, doing nothing.");
00130         }
00131 }
00132 
00133 
00134 // set global plan to wrapper and pass it to eband
00135 bool EBandPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
00136 {
00137     
00138         // check if plugin initialized
00139         if(!initialized_)
00140         {
00141                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00142                 return false;
00143         }
00144 
00145     //reset the global plan
00146     global_plan_.clear();
00147     global_plan_ = orig_global_plan;
00148         
00149         // transform global plan to the map frame we are working in
00150         // this also cuts the plan off (reduces it to local window)
00151         std::vector<int> start_end_counts (2, (int) global_plan_.size()); // counts from the end() of the plan
00152         if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, start_end_counts))
00153         {
00154                 // if plan could not be tranformed abort control and local planning
00155                 ROS_WARN("Could not transform the global plan to the frame of the controller");
00156                 return false;
00157         }
00158 
00159     // also check if there really is a plan
00160         if(transformed_plan_.empty())
00161         {
00162                 // if global plan passed in is empty... we won't do anything
00163                 ROS_WARN("Transformed plan is empty. Aborting local planner!");
00164                 return false;
00165         }
00166 
00167 
00168         // set plan - as this is fresh from the global planner robot pose should be identical to start frame
00169         if(!eband_->setPlan(transformed_plan_))
00170         {
00171                 ROS_ERROR("Setting plan to Elastic Band method failed!");
00172                 return false;
00173         }
00174         ROS_INFO("Global plan set to elastic band for optimization");
00175 
00176         // plan transformed and set to elastic band successfully - set counters to global variable
00177         plan_start_end_counter_ = start_end_counts;
00178 
00179         // let eband refine the plan before starting continuous operation (to smooth sampling based plans)
00180         eband_->optimizeBand();
00181 
00182 
00183         // display result
00184         std::vector<eband_local_planner::Bubble> current_band;
00185         if(eband_->getBand(current_band))
00186                 eband_visual_->publishBand("bubbles", current_band);
00187 
00188 
00189     return true;
00190 }
00191 
00192 
00193 bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
00194 {       
00195         // check if plugin initialized
00196         if(!initialized_)
00197         {
00198                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00199                 return false;
00200         }
00201 
00202         // instantiate local variables
00203         //std::vector<geometry_msgs::PoseStamped> local_plan;
00204         tf::Stamped<tf::Pose> global_pose;
00205         geometry_msgs::PoseStamped global_pose_msg;
00206         std::vector<geometry_msgs::PoseStamped> tmp_plan;
00207 
00208         // get curent robot position
00209         ROS_DEBUG("Reading current robot Position from costmap and appending it to elastic band.");
00210         if(!costmap_ros_->getRobotPose(global_pose))
00211         {
00212                 ROS_WARN("Could not retrieve up to date robot pose from costmap for local planning.");
00213                 return false;
00214         }
00215 
00216         // convert robot pose to frame in plan and set position in band at which to append
00217         tf::poseStampedTFToMsg(global_pose, global_pose_msg);
00218         tmp_plan.assign(1, global_pose_msg);
00219         eband_local_planner::AddAtPosition add_frames_at = add_front;
00220 
00221         // set it to elastic band and let eband connect it
00222         if(!eband_->addFrames(tmp_plan, add_frames_at))
00223         {
00224                 ROS_WARN("Could not connect robot pose to existing elastic band.");
00225                 return false;
00226         }
00227 
00228 
00229         // get additional path-frames which are now in moving window
00230         ROS_DEBUG("Checking for new path frames in moving window");
00231         std::vector<int> plan_start_end_counter = plan_start_end_counter_;
00232         std::vector<geometry_msgs::PoseStamped> append_transformed_plan;
00233         // transform global plan to the map frame we are working in - careful this also cuts the plan off (reduces it to local window)
00234         if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, plan_start_end_counter))
00235         {
00236                 // if plan could not be tranformed abort control and local planning
00237                 ROS_WARN("Could not transform the global plan to the frame of the controller");
00238                 return false;
00239         }
00240 
00241     // also check if there really is a plan
00242         if(transformed_plan_.empty())
00243         {
00244                 // if global plan passed in is empty... we won't do anything
00245                 ROS_WARN("Transformed plan is empty. Aborting local planner!");
00246                 return false;
00247         }
00248         
00249         ROS_DEBUG("Retrieved start-end-counts are: (%d, %d)", plan_start_end_counter.at(0), plan_start_end_counter.at(1));
00250         ROS_DEBUG("Current start-end-counts are: (%d, %d)", plan_start_end_counter_.at(0), plan_start_end_counter_.at(1));
00251 
00252         // identify new frames - if there are any
00253         append_transformed_plan.clear();
00254         // did last transformed plan end futher away from end of complete plan than this transformed plan?
00255         if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(1)) // counting from the back (as start might be pruned)
00256         {
00257                 // new frames in moving window
00258                 if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(0)) // counting from the back (as start might be pruned)
00259                 {
00260                         // append everything
00261                         append_transformed_plan = transformed_plan_;
00262                 }
00263                 else
00264                 {
00265                         // append only the new portion of the plan
00266                         int discarded_frames = plan_start_end_counter.at(0) - plan_start_end_counter_.at(1);
00267                         ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 >= transformed_plan_.begin());
00268                         ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 < transformed_plan_.end());
00269                         append_transformed_plan.assign(transformed_plan_.begin() + discarded_frames + 1, transformed_plan_.end());
00270                 }
00271 
00272                 // set it to elastic band and let eband connect it
00273                 ROS_DEBUG("Adding %d new frames to current band", (int) append_transformed_plan.size());
00274                 add_frames_at = add_back;
00275                 if(eband_->addFrames(append_transformed_plan, add_back))
00276                 {
00277                         // appended frames succesfully to global plan - set new start-end counts
00278                         ROS_DEBUG("Sucessfully added frames to band");
00279                         plan_start_end_counter_ = plan_start_end_counter;
00280                 }
00281                 else
00282                         ROS_DEBUG("Failed to add frames to existing band");
00283         }
00284         else
00285                 ROS_DEBUG("Nothing to add");
00286 
00287 
00288         // update Elastic Band (react on obstacle from costmap, ...)
00289         ROS_DEBUG("Calling optimization method for elastic band");
00290         std::vector<eband_local_planner::Bubble> current_band;
00291         if(!eband_->optimizeBand())
00292         {
00293                 ROS_DEBUG("Optimization failed - Band invalid - No controls availlable");
00294                 // display current band
00295                 if(eband_->getBand(current_band))
00296                         eband_visual_->publishBand("bubbles", current_band);
00297                 return false;
00298         }
00299 
00300 
00301         // get current Elastic Band and
00302         eband_->getBand(current_band);
00303         // set it to the controller
00304         if(!eband_trj_ctrl_->setBand(current_band))
00305         {
00306                 ROS_DEBUG("Failed to to set current band to Trajectory Controller");
00307                 return false;
00308         }
00309 
00310         // set Odometry to controller
00311         if(!eband_trj_ctrl_->setOdometry(base_odom_))
00312         {
00313                 ROS_DEBUG("Failed to to set current odometry to Trajectory Controller");
00314                 return false;
00315         }
00316 
00317         // get resulting commands from the controller
00318         geometry_msgs::Twist cmd_twist;
00319         if(!eband_trj_ctrl_->getTwist(cmd_twist))
00320         {
00321                 ROS_DEBUG("Failed to calculate Twist from band in Trajectory Controller");
00322                 return false;
00323         }
00324 
00325 
00326         // set retrieved commands to reference variable
00327         ROS_DEBUG("Retrieving velocity command: (%f, %f, %f)", cmd_twist.linear.x, cmd_twist.linear.y, cmd_twist.angular.z);
00328         cmd_vel = cmd_twist;
00329 
00330 
00331         // publish plan
00332         std::vector<geometry_msgs::PoseStamped> refined_plan;
00333         if(eband_->getPlan(refined_plan))
00334                 // TODO publish local and current gloabl plan
00335                 base_local_planner::publishPlan(refined_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
00336         //base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
00337 
00338         // display current band
00339         if(eband_->getBand(current_band))
00340                 eband_visual_->publishBand("bubbles", current_band);
00341         
00342 
00343         return true;
00344 }
00345 
00346 
00347 bool EBandPlannerROS::isGoalReached()
00348 {
00349         // check if plugin initialized
00350         if(!initialized_)
00351         {
00352                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00353                 return false;
00354         }
00355 
00356         // copy odometry information to local variable
00357         nav_msgs::Odometry base_odom;
00358         {
00359                 // make sure we do not read new date from topic right at the moment
00360                 boost::mutex::scoped_lock lock(odom_mutex_);
00361                 base_odom = base_odom_;
00362         }
00363 
00364         // analogous to dwa_planner the actual check uses the routine implemented in trajectory_planner (trajectory rollout) 
00365         return base_local_planner::isGoalReached(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), base_odom, 
00366                                 rot_stopped_vel_, trans_stopped_vel_, xy_goal_tolerance_, yaw_goal_tolerance_);
00367 }
00368 
00369 
00370 void EBandPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
00371 {
00372         // lock Callback while reading data from topic
00373     boost::mutex::scoped_lock lock(odom_mutex_);
00374 
00375         // get odometry and write it to member variable (we assume that the odometry is published in the frame of the base)
00376         base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00377         base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00378         base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00379 }
00380 
00381 
00382 }
00383 
00384 


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi
autogenerated on Fri Jan 3 2014 11:34:16