00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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
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
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
00136
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
00147
00148
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
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
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
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
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
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
00292
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
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
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
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
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
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
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
00369 global_plan_.clear();
00370 global_plan_ = orig_global_plan;
00371
00372
00373 xy_tolerance_latch_ = false;
00374
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
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
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
00409
00410
00411
00412
00413
00414
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
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
00429 if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
00430
00431
00432
00433 if (latch_xy_goal_tolerance_) {
00434 xy_tolerance_latch_ = true;
00435 }
00436
00437 double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
00438
00439 if (fabs(angle) <= yaw_goal_tolerance_) {
00440
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
00449
00450 tc_->updatePlan(transformed_plan);
00451 Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
00452 map_viz_.publishCostCloud(costmap_);
00453
00454
00455 nav_msgs::Odometry base_odom;
00456 odom_helper_.getOdom(base_odom);
00457
00458
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
00465 else{
00466
00467 rotating_to_goal_ = true;
00468 if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) {
00469 return false;
00470 }
00471 }
00472 }
00473
00474
00475 publishPlan(transformed_plan, g_plan_pub_);
00476 publishPlan(local_plan, l_plan_pub_);
00477
00478
00479 return true;
00480 }
00481
00482 tc_->updatePlan(transformed_plan);
00483
00484
00485 Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
00486
00487 map_viz_.publishCostCloud(costmap_);
00488
00489
00490
00491
00492
00493
00494
00495
00496
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
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
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
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
00540
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
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
00568 tf::Stamped<tf::Pose> global_pose;
00569 if(costmap_ros_->getRobotPose(global_pose)){
00570 if(update_map){
00571
00572
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
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
00603 return reached_goal_;
00604 }
00605 };