dwa_planner_ros.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 <dwa_local_planner/dwa_planner_ros.h>
00039 #include <Eigen/Core>
00040 #include <cmath>
00041 
00042 #include <ros/console.h>
00043 
00044 #include <pluginlib/class_list_macros.h>
00045 
00046 #include <base_local_planner/goal_functions.h>
00047 #include <nav_msgs/Path.h>
00048 
00049 //register this planner as a BaseLocalPlanner plugin
00050 PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)
00051 
00052 namespace dwa_local_planner {
00053 
00054   void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {
00055       if (setup_ && config.restore_defaults) {
00056         config = default_config_;
00057         config.restore_defaults = false;
00058       }
00059       if ( ! setup_) {
00060         default_config_ = config;
00061         setup_ = true;
00062       }
00063 
00064       // update generic local planner params
00065       base_local_planner::LocalPlannerLimits limits;
00066       limits.max_trans_vel = config.max_trans_vel;
00067       limits.min_trans_vel = config.min_trans_vel;
00068       limits.max_vel_x = config.max_vel_x;
00069       limits.min_vel_x = config.min_vel_x;
00070       limits.max_vel_y = config.max_vel_y;
00071       limits.min_vel_y = config.min_vel_y;
00072       limits.max_rot_vel = config.max_rot_vel;
00073       limits.min_rot_vel = config.min_rot_vel;
00074       limits.acc_lim_x = config.acc_lim_x;
00075       limits.acc_lim_y = config.acc_lim_y;
00076       limits.acc_lim_theta = config.acc_lim_theta;
00077       limits.acc_limit_trans = config.acc_limit_trans;
00078       limits.xy_goal_tolerance = config.xy_goal_tolerance;
00079       limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
00080       limits.prune_plan = config.prune_plan;
00081       limits.trans_stopped_vel = config.trans_stopped_vel;
00082       limits.rot_stopped_vel = config.rot_stopped_vel;
00083       planner_util_.reconfigureCB(limits, config.restore_defaults);
00084 
00085       // update dwa specific configuration
00086       dp_->reconfigure(config);
00087   }
00088 
00089   DWAPlannerROS::DWAPlannerROS() : initialized_(false),
00090       odom_helper_("odom"), setup_(false) {
00091 
00092   }
00093 
00094   void DWAPlannerROS::initialize(
00095       std::string name,
00096       tf::TransformListener* tf,
00097       costmap_2d::Costmap2DROS* costmap_ros) {
00098     if (! isInitialized()) {
00099 
00100       ros::NodeHandle private_nh("~/" + name);
00101       g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
00102       l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
00103       tf_ = tf;
00104       costmap_ros_ = costmap_ros;
00105       costmap_ros_->getRobotPose(current_pose_);
00106 
00107       // make sure to update the costmap we'll use for this cycle
00108       costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00109 
00110       planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());
00111 
00112       //create the actual planner that we'll use.. it'll configure itself from the parameter server
00113       dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
00114 
00115       if( private_nh.getParam( "odom_topic", odom_topic_ ))
00116       {
00117         odom_helper_.setOdomTopic( odom_topic_ );
00118       }
00119       
00120       initialized_ = true;
00121 
00122       dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
00123       dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
00124       dsrv_->setCallback(cb);
00125     }
00126     else{
00127       ROS_WARN("This planner has already been initialized, doing nothing.");
00128     }
00129   }
00130   
00131   bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
00132     if (! isInitialized()) {
00133       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00134       return false;
00135     }
00136     //when we get a new plan, we also want to clear any latch we may have on goal tolerances
00137     latchedStopRotateController_.resetLatching();
00138 
00139     ROS_INFO("Got new plan");
00140     return dp_->setPlan(orig_global_plan);
00141   }
00142 
00143   bool DWAPlannerROS::isGoalReached() {
00144     if (! isInitialized()) {
00145       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00146       return false;
00147     }
00148     if ( ! costmap_ros_->getRobotPose(current_pose_)) {
00149       ROS_ERROR("Could not get robot pose");
00150       return false;
00151     }
00152 
00153     if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
00154       ROS_INFO("Goal reached");
00155       return true;
00156     } else {
00157       return false;
00158     }
00159   }
00160 
00161   void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
00162     base_local_planner::publishPlan(path, l_plan_pub_);
00163   }
00164 
00165 
00166   void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
00167     base_local_planner::publishPlan(path, g_plan_pub_);
00168   }
00169 
00170   DWAPlannerROS::~DWAPlannerROS(){
00171     //make sure to clean things up
00172     delete dsrv_;
00173   }
00174 
00175 
00176 
00177   bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) {
00178     // dynamic window sampling approach to get useful velocity commands
00179     if(! isInitialized()){
00180       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00181       return false;
00182     }
00183 
00184     tf::Stamped<tf::Pose> robot_vel;
00185     odom_helper_.getRobotVel(robot_vel);
00186 
00187     /* For timing uncomment
00188     struct timeval start, end;
00189     double start_t, end_t, t_diff;
00190     gettimeofday(&start, NULL);
00191     */
00192 
00193     //compute what trajectory to drive along
00194     tf::Stamped<tf::Pose> drive_cmds;
00195     drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
00196     
00197     // call with updated footprint
00198     base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
00199     //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
00200 
00201     /* For timing uncomment
00202     gettimeofday(&end, NULL);
00203     start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00204     end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00205     t_diff = end_t - start_t;
00206     ROS_INFO("Cycle time: %.9f", t_diff);
00207     */
00208 
00209     //pass along drive commands
00210     cmd_vel.linear.x = drive_cmds.getOrigin().getX();
00211     cmd_vel.linear.y = drive_cmds.getOrigin().getY();
00212     cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
00213 
00214     //if we cannot move... tell someone
00215     std::vector<geometry_msgs::PoseStamped> local_plan;
00216     if(path.cost_ < 0) {
00217       ROS_DEBUG_NAMED("dwa_local_planner",
00218           "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
00219       local_plan.clear();
00220       publishLocalPlan(local_plan);
00221       return false;
00222     }
00223 
00224     ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", 
00225                     cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
00226 
00227     // Fill out the local plan
00228     for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
00229       double p_x, p_y, p_th;
00230       path.getPoint(i, p_x, p_y, p_th);
00231 
00232       tf::Stamped<tf::Pose> p =
00233               tf::Stamped<tf::Pose>(tf::Pose(
00234                       tf::createQuaternionFromYaw(p_th),
00235                       tf::Point(p_x, p_y, 0.0)),
00236                       ros::Time::now(),
00237                       costmap_ros_->getGlobalFrameID());
00238       geometry_msgs::PoseStamped pose;
00239       tf::poseStampedTFToMsg(p, pose);
00240       local_plan.push_back(pose);
00241     }
00242 
00243     //publish information to the visualizer
00244 
00245     publishLocalPlan(local_plan);
00246     return true;
00247   }
00248 
00249 
00250 
00251 
00252   bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
00253     // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
00254     if ( ! costmap_ros_->getRobotPose(current_pose_)) {
00255       ROS_ERROR("Could not get robot pose");
00256       return false;
00257     }
00258     std::vector<geometry_msgs::PoseStamped> transformed_plan;
00259     if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
00260       ROS_ERROR("Could not get local plan");
00261       return false;
00262     }
00263 
00264     //if the global plan passed in is empty... we won't do anything
00265     if(transformed_plan.empty()) {
00266       ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
00267       return false;
00268     }
00269     ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
00270 
00271     // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
00272     dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan);
00273 
00274     if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
00275       //publish an empty plan because we've reached our goal position
00276       std::vector<geometry_msgs::PoseStamped> local_plan;
00277       std::vector<geometry_msgs::PoseStamped> transformed_plan;
00278       publishGlobalPlan(transformed_plan);
00279       publishLocalPlan(local_plan);
00280       base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
00281       return latchedStopRotateController_.computeVelocityCommandsStopRotate(
00282           cmd_vel,
00283           limits.getAccLimits(),
00284           dp_->getSimPeriod(),
00285           &planner_util_,
00286           odom_helper_,
00287           current_pose_,
00288           boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
00289     } else {
00290       bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
00291       if (isOk) {
00292         publishGlobalPlan(transformed_plan);
00293       } else {
00294         ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
00295         std::vector<geometry_msgs::PoseStamped> empty_plan;
00296         publishGlobalPlan(empty_plan);
00297       }
00298       return isOk;
00299     }
00300   }
00301 
00302 
00303 };


dwa_local_planner
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:59