dwa_planner.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 #include <dwa_local_planner/dwa_planner.h>
00038 #include <base_local_planner/goal_functions.h>
00039 #include <base_local_planner/map_grid_cost_point.h>
00040 #include <cmath>
00041 
00042 //for computing path distance
00043 #include <queue>
00044 
00045 #include <angles/angles.h>
00046 
00047 #include <ros/ros.h>
00048 
00049 #include <pcl_conversions/pcl_conversions.h>
00050 
00051 namespace dwa_local_planner {
00052   void DWAPlanner::reconfigure(DWAPlannerConfig &config)
00053   {
00054 
00055     boost::mutex::scoped_lock l(configuration_mutex_);
00056 
00057     generator_.setParameters(
00058         config.sim_time,
00059         config.sim_granularity,
00060         config.angular_sim_granularity,
00061         config.use_dwa,
00062         sim_period_);
00063 
00064     double resolution = planner_util_->getCostmap()->getResolution();
00065     pdist_scale_ = config.path_distance_bias;
00066     // pdistscale used for both path and alignment, set  forward_point_distance to zero to discard alignment
00067     path_costs_.setScale(resolution * pdist_scale_ * 0.5);
00068     alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
00069 
00070     gdist_scale_ = config.goal_distance_bias;
00071     goal_costs_.setScale(resolution * gdist_scale_ * 0.5);
00072     goal_front_costs_.setScale(resolution * gdist_scale_ * 0.5);
00073 
00074     occdist_scale_ = config.occdist_scale;
00075     obstacle_costs_.setScale(resolution * occdist_scale_);
00076 
00077     stop_time_buffer_ = config.stop_time_buffer;
00078     oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
00079     forward_point_distance_ = config.forward_point_distance;
00080     goal_front_costs_.setXShift(forward_point_distance_);
00081     alignment_costs_.setXShift(forward_point_distance_);
00082  
00083     // obstacle costs can vary due to scaling footprint feature
00084     obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
00085 
00086     int vx_samp, vy_samp, vth_samp;
00087     vx_samp = config.vx_samples;
00088     vy_samp = config.vy_samples;
00089     vth_samp = config.vth_samples;
00090  
00091     if (vx_samp <= 0) {
00092       ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
00093       vx_samp = 1;
00094       config.vx_samples = vx_samp;
00095     }
00096  
00097     if (vy_samp <= 0) {
00098       ROS_WARN("You've specified that you don't want any samples in the y dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
00099       vy_samp = 1;
00100       config.vy_samples = vy_samp;
00101     }
00102  
00103     if (vth_samp <= 0) {
00104       ROS_WARN("You've specified that you don't want any samples in the th dimension. We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");
00105       vth_samp = 1;
00106       config.vth_samples = vth_samp;
00107     }
00108  
00109     vsamples_[0] = vx_samp;
00110     vsamples_[1] = vy_samp;
00111     vsamples_[2] = vth_samp;
00112  
00113 
00114   }
00115 
00116   DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
00117       planner_util_(planner_util),
00118       obstacle_costs_(planner_util->getCostmap()),
00119       path_costs_(planner_util->getCostmap()),
00120       goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
00121       goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
00122       alignment_costs_(planner_util->getCostmap())
00123   {
00124     ros::NodeHandle private_nh("~/" + name);
00125 
00126     goal_front_costs_.setStopOnFailure( false );
00127     alignment_costs_.setStopOnFailure( false );
00128 
00129     //Assuming this planner is being run within the navigation stack, we can
00130     //just do an upward search for the frequency at which its being run. This
00131     //also allows the frequency to be overwritten locally.
00132     std::string controller_frequency_param_name;
00133     if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
00134       sim_period_ = 0.05;
00135     } else {
00136       double controller_frequency = 0;
00137       private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
00138       if(controller_frequency > 0) {
00139         sim_period_ = 1.0 / controller_frequency;
00140       } else {
00141         ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
00142         sim_period_ = 0.05;
00143       }
00144     }
00145     ROS_INFO("Sim period is set to %.2f", sim_period_);
00146 
00147     oscillation_costs_.resetOscillationFlags();
00148 
00149     bool sum_scores;
00150     private_nh.param("sum_scores", sum_scores, false);
00151     obstacle_costs_.setSumScores(sum_scores);
00152 
00153 
00154     private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
00155     map_viz_.initialize(name, planner_util->getGlobalFrame(), boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
00156 
00157     std::string frame_id;
00158     private_nh.param("global_frame_id", frame_id, std::string("odom"));
00159 
00160     traj_cloud_ = new pcl::PointCloud<base_local_planner::MapGridCostPoint>;
00161     traj_cloud_->header.frame_id = frame_id;
00162     traj_cloud_pub_.advertise(private_nh, "trajectory_cloud", 1);
00163     private_nh.param("publish_traj_pc", publish_traj_pc_, false);
00164 
00165     // set up all the cost functions that will be applied in order
00166     // (any function returning negative values will abort scoring, so the order can improve performance)
00167     std::vector<base_local_planner::TrajectoryCostFunction*> critics;
00168     critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
00169     critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
00170     critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
00171     critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
00172     critics.push_back(&path_costs_); // prefers trajectories on global path
00173     critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
00174 
00175     // trajectory generators
00176     std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
00177     generator_list.push_back(&generator_);
00178 
00179     scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
00180 
00181     private_nh.param("cheat_factor", cheat_factor_, 1.0);
00182   }
00183 
00184   // used for visualization only, total_costs are not really total costs
00185   bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
00186 
00187     path_cost = path_costs_.getCellCosts(cx, cy);
00188     goal_cost = goal_costs_.getCellCosts(cx, cy);
00189     occ_cost = planner_util_->getCostmap()->getCost(cx, cy);
00190     if (path_cost == path_costs_.obstacleCosts() ||
00191         path_cost == path_costs_.unreachableCellCosts() ||
00192         occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
00193       return false;
00194     }
00195 
00196     double resolution = planner_util_->getCostmap()->getResolution();
00197     total_cost =
00198         pdist_scale_ * resolution * path_cost +
00199         gdist_scale_ * resolution * goal_cost +
00200         occdist_scale_ * occ_cost;
00201     return true;
00202   }
00203 
00204   bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
00205     oscillation_costs_.resetOscillationFlags();
00206     return planner_util_->setPlan(orig_global_plan);
00207   }
00208 
00213   bool DWAPlanner::checkTrajectory(
00214       Eigen::Vector3f pos,
00215       Eigen::Vector3f vel,
00216       Eigen::Vector3f vel_samples){
00217     oscillation_costs_.resetOscillationFlags();
00218     base_local_planner::Trajectory traj;
00219     geometry_msgs::PoseStamped goal_pose = global_plan_.back();
00220     Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
00221     base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
00222     generator_.initialise(pos,
00223         vel,
00224         goal,
00225         &limits,
00226         vsamples_);
00227     generator_.generateTrajectory(pos, vel, vel_samples, traj);
00228     double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
00229     //if the trajectory is a legal one... the check passes
00230     if(cost >= 0) {
00231       return true;
00232     }
00233     ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
00234 
00235     //otherwise the check fails
00236     return false;
00237   }
00238 
00239 
00240   void DWAPlanner::updatePlanAndLocalCosts(
00241       tf::Stamped<tf::Pose> global_pose,
00242       const std::vector<geometry_msgs::PoseStamped>& new_plan) {
00243     global_plan_.resize(new_plan.size());
00244     for (unsigned int i = 0; i < new_plan.size(); ++i) {
00245       global_plan_[i] = new_plan[i];
00246     }
00247 
00248     // costs for going away from path
00249     path_costs_.setTargetPoses(global_plan_);
00250 
00251     // costs for not going towards the local goal as much as possible
00252     goal_costs_.setTargetPoses(global_plan_);
00253 
00254     // alignment costs
00255     geometry_msgs::PoseStamped goal_pose = global_plan_.back();
00256 
00257     Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
00258     double sq_dist =
00259         (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
00260         (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
00261 
00262     // we want the robot nose to be drawn to its final position
00263     // (before robot turns towards goal orientation), not the end of the
00264     // path for the robot center. Choosing the final position after
00265     // turning towards goal orientation causes instability when the
00266     // robot needs to make a 180 degree turn at the end
00267     std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
00268     double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
00269     front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
00270       forward_point_distance_ * cos(angle_to_goal);
00271     front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
00272       sin(angle_to_goal);
00273 
00274     goal_front_costs_.setTargetPoses(front_global_plan);
00275     
00276     // keeping the nose on the path
00277     if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
00278       double resolution = planner_util_->getCostmap()->getResolution();
00279       alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
00280       // costs for robot being aligned with path (nose on path, not ju
00281       alignment_costs_.setTargetPoses(global_plan_);
00282     } else {
00283       // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
00284       alignment_costs_.setScale(0.0);
00285     }
00286   }
00287 
00288 
00289   /*
00290    * given the current state of the robot, find a good trajectory
00291    */
00292   base_local_planner::Trajectory DWAPlanner::findBestPath(
00293       tf::Stamped<tf::Pose> global_pose,
00294       tf::Stamped<tf::Pose> global_vel,
00295       tf::Stamped<tf::Pose>& drive_velocities,
00296       std::vector<geometry_msgs::Point> footprint_spec) {
00297 
00298     obstacle_costs_.setFootprint(footprint_spec);
00299 
00300     //make sure that our configuration doesn't change mid-run
00301     boost::mutex::scoped_lock l(configuration_mutex_);
00302 
00303     Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
00304     Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
00305     geometry_msgs::PoseStamped goal_pose = global_plan_.back();
00306     Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
00307     base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
00308 
00309     // prepare cost functions and generators for this run
00310     generator_.initialise(pos,
00311         vel,
00312         goal,
00313         &limits,
00314         vsamples_);
00315 
00316     result_traj_.cost_ = -7;
00317     // find best trajectory by sampling and scoring the samples
00318     std::vector<base_local_planner::Trajectory> all_explored;
00319     scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
00320 
00321     if(publish_traj_pc_)
00322     {
00323         base_local_planner::MapGridCostPoint pt;
00324         traj_cloud_->points.clear();
00325         traj_cloud_->width = 0;
00326         traj_cloud_->height = 0;
00327         std_msgs::Header header;
00328         pcl_conversions::fromPCL(traj_cloud_->header, header);
00329         header.stamp = ros::Time::now();
00330         traj_cloud_->header = pcl_conversions::toPCL(header);
00331         for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
00332         {
00333             if(t->cost_<0)
00334                 continue;
00335             // Fill out the plan
00336             for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
00337                 double p_x, p_y, p_th;
00338                 t->getPoint(i, p_x, p_y, p_th);
00339                 pt.x=p_x;
00340                 pt.y=p_y;
00341                 pt.z=0;
00342                 pt.path_cost=p_th;
00343                 pt.total_cost=t->cost_;
00344                 traj_cloud_->push_back(pt);
00345             }
00346         }
00347         traj_cloud_pub_.publish(*traj_cloud_);
00348     }
00349 
00350     // verbose publishing of point clouds
00351     if (publish_cost_grid_pc_) {
00352       //we'll publish the visualization of the costs to rviz before returning our best trajectory
00353       map_viz_.publishCostCloud(planner_util_->getCostmap());
00354     }
00355 
00356     // debrief stateful scoring functions
00357     oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
00358 
00359     //if we don't have a legal trajectory, we'll just command zero
00360     if (result_traj_.cost_ < 0) {
00361       drive_velocities.setIdentity();
00362     } else {
00363       tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
00364       drive_velocities.setOrigin(start);
00365       tf::Matrix3x3 matrix;
00366       matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
00367       drive_velocities.setBasis(matrix);
00368     }
00369 
00370     return result_traj_;
00371   }
00372 };


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