trajectory_planner_ros.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, 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 the Willow Garage 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 <base_local_planner/trajectory_planner_ros.h>
00039 
00040 #include <sys/time.h>
00041 #include <boost/tokenizer.hpp>
00042 
00043 #include <Eigen/Core>
00044 #include <cmath>
00045 
00046 #include <ros/console.h>
00047 
00048 #include <pluginlib/class_list_macros.h>
00049 
00050 #include <base_local_planner/goal_functions.h>
00051 #include <nav_msgs/Path.h>
00052 
00053 
00054 
00055 //register this planner as a BaseLocalPlanner plugin
00056 PLUGINLIB_EXPORT_CLASS(base_local_planner::TrajectoryPlannerROS, nav_core::BaseLocalPlanner)
00057 
00058 namespace base_local_planner {
00059 
00060   void TrajectoryPlannerROS::reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level) {
00061       if (setup_ && config.restore_defaults) {
00062         config = default_config_;
00063         //Avoid looping
00064         config.restore_defaults = false;
00065       }
00066       if ( ! setup_) {
00067         default_config_ = config;
00068         setup_ = true;
00069       }
00070       tc_->reconfigure(config);
00071       reached_goal_ = false;
00072   }
00073 
00074   TrajectoryPlannerROS::TrajectoryPlannerROS() :
00075       world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {}
00076 
00077   TrajectoryPlannerROS::TrajectoryPlannerROS(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) :
00078       world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {
00079 
00080       //initialize the planner
00081       initialize(name, tf, costmap_ros);
00082   }
00083 
00084   void TrajectoryPlannerROS::initialize(
00085       std::string name,
00086       tf::TransformListener* tf,
00087       costmap_2d::Costmap2DROS* costmap_ros){
00088     if (! isInitialized()) {
00089 
00090       ros::NodeHandle private_nh("~/" + name);
00091       g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
00092       l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
00093 
00094 
00095       tf_ = tf;
00096       costmap_ros_ = costmap_ros;
00097       rot_stopped_velocity_ = 1e-2;
00098       trans_stopped_velocity_ = 1e-2;
00099       double sim_time, sim_granularity, angular_sim_granularity;
00100       int vx_samples, vtheta_samples;
00101       double pdist_scale, gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta;
00102       bool holonomic_robot, dwa, simple_attractor, heading_scoring;
00103       double heading_scoring_timestep;
00104       double max_vel_x, min_vel_x;
00105       double backup_vel;
00106       double stop_time_buffer;
00107       std::string world_model_type;
00108       rotating_to_goal_ = false;
00109 
00110       //initialize the copy of the costmap the controller will use
00111       costmap_ = costmap_ros_->getCostmap();
00112 
00113 
00114       global_frame_ = costmap_ros_->getGlobalFrameID();
00115       robot_base_frame_ = costmap_ros_->getBaseFrameID();
00116       private_nh.param("prune_plan", prune_plan_, true);
00117 
00118       private_nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
00119       private_nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
00120       private_nh.param("acc_lim_x", acc_lim_x_, 2.5);
00121       private_nh.param("acc_lim_y", acc_lim_y_, 2.5);
00122       //this was improperly set as acc_lim_th -- TODO: remove this when we get to J turtle
00123       acc_lim_theta_ = 3.2;
00124       if (private_nh.hasParam("acc_lim_th"))
00125       {
00126         ROS_WARN("%s/acc_lim_th should be acc_lim_theta, this param will be removed in J-turtle", private_nh.getNamespace().c_str());
00127         private_nh.param("acc_lim_th", acc_lim_theta_, 3.2);
00128       }
00129       private_nh.param("acc_lim_theta", acc_lim_theta_, acc_lim_theta_);
00130 
00131       private_nh.param("stop_time_buffer", stop_time_buffer, 0.2);
00132 
00133       private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
00134 
00135       //Since I screwed up nicely in my documentation, I'm going to add errors
00136       //informing the user if they've set one of the wrong parameters
00137       if(private_nh.hasParam("acc_limit_x"))
00138         ROS_ERROR("You are using acc_limit_x where you should be using acc_lim_x. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
00139 
00140       if(private_nh.hasParam("acc_limit_y"))
00141         ROS_ERROR("You are using acc_limit_y where you should be using acc_lim_y. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
00142 
00143       if(private_nh.hasParam("acc_limit_th"))
00144         ROS_ERROR("You are using acc_limit_th where you should be using acc_lim_th. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
00145 
00146       //Assuming this planner is being run within the navigation stack, we can
00147       //just do an upward search for the frequency at which its being run. This
00148       //also allows the frequency to be overwritten locally.
00149       std::string controller_frequency_param_name;
00150       if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name))
00151         sim_period_ = 0.05;
00152       else
00153       {
00154         double controller_frequency = 0;
00155         private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
00156         if(controller_frequency > 0)
00157           sim_period_ = 1.0 / controller_frequency;
00158         else
00159         {
00160           ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
00161           sim_period_ = 0.05;
00162         }
00163       }
00164       ROS_INFO("Sim period is set to %.2f", sim_period_);
00165 
00166       private_nh.param("sim_time", sim_time, 1.0);
00167       private_nh.param("sim_granularity", sim_granularity, 0.025);
00168       private_nh.param("angular_sim_granularity", angular_sim_granularity, sim_granularity);
00169       private_nh.param("vx_samples", vx_samples, 3);
00170       private_nh.param("vtheta_samples", vtheta_samples, 20);
00171 
00172       private_nh.param("path_distance_bias", pdist_scale, 0.6);
00173       private_nh.param("goal_distance_bias", gdist_scale, 0.8);
00174       private_nh.param("occdist_scale", occdist_scale, 0.01);
00175 
00176       bool meter_scoring;
00177       if ( ! private_nh.hasParam("meter_scoring")) {
00178         ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring not set. Set it to true to make your settins robust against changes of costmap resolution.");
00179       } else {
00180         private_nh.param("meter_scoring", meter_scoring, false);
00181 
00182         if(meter_scoring) {
00183           //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
00184           double resolution = costmap_->getResolution();
00185           gdist_scale *= resolution;
00186           pdist_scale *= resolution;
00187           occdist_scale *= resolution;
00188         } else {
00189           ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring set to false. Set it to true to make your settins robust against changes of costmap resolution.");
00190         }
00191       }
00192 
00193       private_nh.param("heading_lookahead", heading_lookahead, 0.325);
00194       private_nh.param("oscillation_reset_dist", oscillation_reset_dist, 0.05);
00195       private_nh.param("escape_reset_dist", escape_reset_dist, 0.10);
00196       private_nh.param("escape_reset_theta", escape_reset_theta, M_PI_4);
00197       private_nh.param("holonomic_robot", holonomic_robot, true);
00198       private_nh.param("max_vel_x", max_vel_x, 0.5);
00199       private_nh.param("min_vel_x", min_vel_x, 0.1);
00200 
00201       double max_rotational_vel;
00202       private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
00203       max_vel_th_ = max_rotational_vel;
00204       min_vel_th_ = -1.0 * max_rotational_vel;
00205       private_nh.param("min_in_place_rotational_vel", min_in_place_vel_th_, 0.4);
00206       reached_goal_ = false;
00207       backup_vel = -0.1;
00208       if(private_nh.getParam("backup_vel", backup_vel))
00209         ROS_WARN("The backup_vel parameter has been deprecated in favor of the escape_vel parameter. To switch, just change the parameter name in your configuration files.");
00210 
00211       //if both backup_vel and escape_vel are set... we'll use escape_vel
00212       private_nh.getParam("escape_vel", backup_vel);
00213 
00214       if(backup_vel >= 0.0)
00215         ROS_WARN("You've specified a positive escape velocity. This is probably not what you want and will cause the robot to move forward instead of backward. You should probably change your escape_vel parameter to be negative");
00216 
00217       private_nh.param("world_model", world_model_type, std::string("costmap"));
00218       private_nh.param("dwa", dwa, true);
00219       private_nh.param("heading_scoring", heading_scoring, false);
00220       private_nh.param("heading_scoring_timestep", heading_scoring_timestep, 0.8);
00221 
00222       simple_attractor = false;
00223 
00224       //parameters for using the freespace controller
00225       double min_pt_separation, max_obstacle_height, grid_resolution;
00226       private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
00227       private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
00228       private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
00229       private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
00230 
00231       ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
00232       world_model_ = new CostmapModel(*costmap_);
00233       std::vector<double> y_vels = loadYVels(private_nh);
00234 
00235       footprint_spec_ = costmap_ros_->getRobotFootprint();
00236 
00237       tc_ = new TrajectoryPlanner(*world_model_, *costmap_, footprint_spec_,
00238           acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
00239           gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
00240           max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,
00241           dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);
00242 
00243       map_viz_.initialize(name, global_frame_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6));
00244       initialized_ = true;
00245 
00246       dsrv_ = new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
00247       dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2);
00248       dsrv_->setCallback(cb);
00249 
00250     } else {
00251       ROS_WARN("This planner has already been initialized, doing nothing");
00252     }
00253   }
00254 
00255   std::vector<double> TrajectoryPlannerROS::loadYVels(ros::NodeHandle node){
00256     std::vector<double> y_vels;
00257 
00258     std::string y_vel_list;
00259     if(node.getParam("y_vels", y_vel_list)){
00260       typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
00261       boost::char_separator<char> sep("[], ");
00262       tokenizer tokens(y_vel_list, sep);
00263 
00264       for(tokenizer::iterator i = tokens.begin(); i != tokens.end(); i++){
00265         y_vels.push_back(atof((*i).c_str()));
00266       }
00267     }
00268     else{
00269       //if no values are passed in, we'll provide defaults
00270       y_vels.push_back(-0.3);
00271       y_vels.push_back(-0.1);
00272       y_vels.push_back(0.1);
00273       y_vels.push_back(0.3);
00274     }
00275 
00276     return y_vels;
00277   }
00278 
00279   TrajectoryPlannerROS::~TrajectoryPlannerROS() {
00280     //make sure to clean things up
00281     delete dsrv_;
00282 
00283     if(tc_ != NULL)
00284       delete tc_;
00285 
00286     if(world_model_ != NULL)
00287       delete world_model_;
00288   }
00289 
00290   bool TrajectoryPlannerROS::stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, geometry_msgs::Twist& cmd_vel){
00291     //slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
00292     //but we'll use a tenth of a second to be consistent with the implementation of the local planner.
00293     double vx = sign(robot_vel.getOrigin().x()) * std::max(0.0, (fabs(robot_vel.getOrigin().x()) - acc_lim_x_ * sim_period_));
00294     double vy = sign(robot_vel.getOrigin().y()) * std::max(0.0, (fabs(robot_vel.getOrigin().y()) - acc_lim_y_ * sim_period_));
00295 
00296     double vel_yaw = tf::getYaw(robot_vel.getRotation());
00297     double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim_theta_ * sim_period_));
00298 
00299     //we do want to check whether or not the command is valid
00300     double yaw = tf::getYaw(global_pose.getRotation());
00301     bool valid_cmd = tc_->checkTrajectory(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw, 
00302         robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw, vx, vy, vth);
00303 
00304     //if we have a valid command, we'll pass it on, otherwise we'll command all zeros
00305     if(valid_cmd){
00306       ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
00307       cmd_vel.linear.x = vx;
00308       cmd_vel.linear.y = vy;
00309       cmd_vel.angular.z = vth;
00310       return true;
00311     }
00312 
00313     cmd_vel.linear.x = 0.0;
00314     cmd_vel.linear.y = 0.0;
00315     cmd_vel.angular.z = 0.0;
00316     return false;
00317   }
00318 
00319   bool TrajectoryPlannerROS::rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel){
00320     double yaw = tf::getYaw(global_pose.getRotation());
00321     double vel_yaw = tf::getYaw(robot_vel.getRotation());
00322     cmd_vel.linear.x = 0;
00323     cmd_vel.linear.y = 0;
00324     double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
00325 
00326     double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_,
00327         std::max(min_in_place_vel_th_, ang_diff)) : std::max(min_vel_th_,
00328         std::min(-1.0 * min_in_place_vel_th_, ang_diff));
00329 
00330     //take the acceleration limits of the robot into account
00331     double max_acc_vel = fabs(vel_yaw) + acc_lim_theta_ * sim_period_;
00332     double min_acc_vel = fabs(vel_yaw) - acc_lim_theta_ * sim_period_;
00333 
00334     v_theta_samp = sign(v_theta_samp) * std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
00335 
00336     //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
00337     double max_speed_to_stop = sqrt(2 * acc_lim_theta_ * fabs(ang_diff)); 
00338 
00339     v_theta_samp = sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp));
00340 
00341     // Re-enforce min_in_place_vel_th_.  It is more important than the acceleration limits.
00342     v_theta_samp = v_theta_samp > 0.0
00343       ? std::min( max_vel_th_, std::max( min_in_place_vel_th_, v_theta_samp ))
00344       : std::max( min_vel_th_, std::min( -1.0 * min_in_place_vel_th_, v_theta_samp ));
00345 
00346     //we still want to lay down the footprint of the robot and check if the action is legal
00347     bool valid_cmd = tc_->checkTrajectory(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw, 
00348         robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw, 0.0, 0.0, v_theta_samp);
00349 
00350     ROS_DEBUG("Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
00351 
00352     if(valid_cmd){
00353       cmd_vel.angular.z = v_theta_samp;
00354       return true;
00355     }
00356 
00357     cmd_vel.angular.z = 0.0;
00358     return false;
00359 
00360   }
00361 
00362   bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
00363     if (! isInitialized()) {
00364       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00365       return false;
00366     }
00367 
00368     //reset the global plan
00369     global_plan_.clear();
00370     global_plan_ = orig_global_plan;
00371     
00372     //when we get a new plan, we also want to clear any latch we may have on goal tolerances
00373     xy_tolerance_latch_ = false;
00374     //reset the at goal flag
00375     reached_goal_ = false;
00376     return true;
00377   }
00378 
00379   bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
00380     if (! isInitialized()) {
00381       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00382       return false;
00383     }
00384 
00385     std::vector<geometry_msgs::PoseStamped> local_plan;
00386     tf::Stamped<tf::Pose> global_pose;
00387     if (!costmap_ros_->getRobotPose(global_pose)) {
00388       return false;
00389     }
00390 
00391     std::vector<geometry_msgs::PoseStamped> transformed_plan;
00392     //get the global plan in our frame
00393     if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
00394       ROS_WARN("Could not transform the global plan to the frame of the controller");
00395       return false;
00396     }
00397 
00398     //now we'll prune the plan based on the position of the robot
00399     if(prune_plan_)
00400       prunePlan(global_pose, transformed_plan, global_plan_);
00401 
00402     tf::Stamped<tf::Pose> drive_cmds;
00403     drive_cmds.frame_id_ = robot_base_frame_;
00404 
00405     tf::Stamped<tf::Pose> robot_vel;
00406     odom_helper_.getRobotVel(robot_vel);
00407 
00408     /* For timing uncomment
00409     struct timeval start, end;
00410     double start_t, end_t, t_diff;
00411     gettimeofday(&start, NULL);
00412     */
00413 
00414     //if the global plan passed in is empty... we won't do anything
00415     if(transformed_plan.empty())
00416       return false;
00417 
00418     tf::Stamped<tf::Pose> goal_point;
00419     tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
00420     //we assume the global goal is the last point in the global plan
00421     double goal_x = goal_point.getOrigin().getX();
00422     double goal_y = goal_point.getOrigin().getY();
00423 
00424     double yaw = tf::getYaw(goal_point.getRotation());
00425 
00426     double goal_th = yaw;
00427 
00428     //check to see if we've reached the goal position
00429     if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
00430 
00431       //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
00432       //just rotate in place
00433       if (latch_xy_goal_tolerance_) {
00434         xy_tolerance_latch_ = true;
00435       }
00436 
00437       double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
00438       //check to see if the goal orientation has been reached
00439       if (fabs(angle) <= yaw_goal_tolerance_) {
00440         //set the velocity command to zero
00441         cmd_vel.linear.x = 0.0;
00442         cmd_vel.linear.y = 0.0;
00443         cmd_vel.angular.z = 0.0;
00444         rotating_to_goal_ = false;
00445         xy_tolerance_latch_ = false;
00446         reached_goal_ = true;
00447       } else {
00448         //we need to call the next two lines to make sure that the trajectory
00449         //planner updates its path distance and goal distance grids
00450         tc_->updatePlan(transformed_plan);
00451         Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
00452         map_viz_.publishCostCloud(costmap_);
00453 
00454         //copy over the odometry information
00455         nav_msgs::Odometry base_odom;
00456         odom_helper_.getOdom(base_odom);
00457 
00458         //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
00459         if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, rot_stopped_velocity_, trans_stopped_velocity_)) {
00460           if ( ! stopWithAccLimits(global_pose, robot_vel, cmd_vel)) {
00461             return false;
00462           }
00463         }
00464         //if we're stopped... then we want to rotate to goal
00465         else{
00466           //set this so that we know its OK to be moving
00467           rotating_to_goal_ = true;
00468           if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) {
00469             return false;
00470           }
00471         }
00472       }
00473 
00474       //publish an empty plan because we've reached our goal position
00475       publishPlan(transformed_plan, g_plan_pub_);
00476       publishPlan(local_plan, l_plan_pub_);
00477 
00478       //we don't actually want to run the controller when we're just rotating to goal
00479       return true;
00480     }
00481 
00482     tc_->updatePlan(transformed_plan);
00483 
00484     //compute what trajectory to drive along
00485     Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
00486 
00487     map_viz_.publishCostCloud(costmap_);
00488     /* For timing uncomment
00489     gettimeofday(&end, NULL);
00490     start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00491     end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00492     t_diff = end_t - start_t;
00493     ROS_INFO("Cycle time: %.9f", t_diff);
00494     */
00495 
00496     //pass along drive commands
00497     cmd_vel.linear.x = drive_cmds.getOrigin().getX();
00498     cmd_vel.linear.y = drive_cmds.getOrigin().getY();
00499     cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
00500 
00501     //if we cannot move... tell someone
00502     if (path.cost_ < 0) {
00503       ROS_DEBUG_NAMED("trajectory_planner_ros",
00504           "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
00505       local_plan.clear();
00506       publishPlan(transformed_plan, g_plan_pub_);
00507       publishPlan(local_plan, l_plan_pub_);
00508       return false;
00509     }
00510 
00511     ROS_DEBUG_NAMED("trajectory_planner_ros", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
00512         cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
00513 
00514     // Fill out the local plan
00515     for (unsigned int i = 0; i < path.getPointsSize(); ++i) {
00516       double p_x, p_y, p_th;
00517       path.getPoint(i, p_x, p_y, p_th);
00518       tf::Stamped<tf::Pose> p =
00519           tf::Stamped<tf::Pose>(tf::Pose(
00520               tf::createQuaternionFromYaw(p_th),
00521               tf::Point(p_x, p_y, 0.0)),
00522               ros::Time::now(),
00523               global_frame_);
00524       geometry_msgs::PoseStamped pose;
00525       tf::poseStampedTFToMsg(p, pose);
00526       local_plan.push_back(pose);
00527     }
00528 
00529     //publish information to the visualizer
00530     publishPlan(transformed_plan, g_plan_pub_);
00531     publishPlan(local_plan, l_plan_pub_);
00532     return true;
00533   }
00534 
00535   bool TrajectoryPlannerROS::checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
00536     tf::Stamped<tf::Pose> global_pose;
00537     if(costmap_ros_->getRobotPose(global_pose)){
00538       if(update_map){
00539         //we need to give the planne some sort of global plan, since we're only checking for legality
00540         //we'll just give the robots current position
00541         std::vector<geometry_msgs::PoseStamped> plan;
00542         geometry_msgs::PoseStamped pose_msg;
00543         tf::poseStampedTFToMsg(global_pose, pose_msg);
00544         plan.push_back(pose_msg);
00545         tc_->updatePlan(plan, true);
00546       }
00547 
00548       //copy over the odometry information
00549       nav_msgs::Odometry base_odom;
00550       {
00551         boost::recursive_mutex::scoped_lock lock(odom_lock_);
00552         base_odom = base_odom_;
00553       }
00554 
00555       return tc_->checkTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
00556           base_odom.twist.twist.linear.x,
00557           base_odom.twist.twist.linear.y,
00558           base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
00559 
00560     }
00561     ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
00562     return false;
00563   }
00564 
00565 
00566   double TrajectoryPlannerROS::scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
00567     // Copy of checkTrajectory that returns a score instead of True / False
00568     tf::Stamped<tf::Pose> global_pose;
00569     if(costmap_ros_->getRobotPose(global_pose)){
00570       if(update_map){
00571         //we need to give the planne some sort of global plan, since we're only checking for legality
00572         //we'll just give the robots current position
00573         std::vector<geometry_msgs::PoseStamped> plan;
00574         geometry_msgs::PoseStamped pose_msg;
00575         tf::poseStampedTFToMsg(global_pose, pose_msg);
00576         plan.push_back(pose_msg);
00577         tc_->updatePlan(plan, true);
00578       }
00579 
00580       //copy over the odometry information
00581       nav_msgs::Odometry base_odom;
00582       {
00583         boost::recursive_mutex::scoped_lock lock(odom_lock_);
00584         base_odom = base_odom_;
00585       }
00586 
00587       return tc_->scoreTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
00588           base_odom.twist.twist.linear.x,
00589           base_odom.twist.twist.linear.y,
00590           base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
00591 
00592     }
00593     ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
00594     return -1.0;
00595   }
00596 
00597   bool TrajectoryPlannerROS::isGoalReached() {
00598     if (! isInitialized()) {
00599       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00600       return false;
00601     }
00602     //return flag set in controller
00603     return reached_goal_; 
00604   }
00605 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08