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/simple_trajectory_generator.h>
00039
00040 #include <cmath>
00041 #include <math.h>
00042
00043 #include <base_local_planner/velocity_iterator.h>
00044
00045 namespace base_local_planner {
00046
00047 void SimpleTrajectoryGenerator::initialise(
00048 const Eigen::Vector3f& pos,
00049 const Eigen::Vector3f& vel,
00050 const Eigen::Vector3f& goal,
00051 base_local_planner::LocalPlannerLimits* limits,
00052 const Eigen::Vector3f& vsamples,
00053 std::vector<Eigen::Vector3f> additional_samples,
00054 bool discretize_by_time) {
00055 initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
00056
00057 sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
00058 }
00059
00060
00061 void SimpleTrajectoryGenerator::initialise(
00062 const Eigen::Vector3f& pos,
00063 const Eigen::Vector3f& vel,
00064 const Eigen::Vector3f& goal,
00065 base_local_planner::LocalPlannerLimits* limits,
00066 const Eigen::Vector3f& vsamples,
00067 bool discretize_by_time) {
00068
00069
00070
00071 double max_vel_th = limits->max_rot_vel;
00072 double min_vel_th = -1.0 * max_vel_th;
00073 discretize_by_time_ = discretize_by_time;
00074 Eigen::Vector3f acc_lim = limits->getAccLimits();
00075 pos_ = pos;
00076 vel_ = vel;
00077 limits_ = limits;
00078 next_sample_index_ = 0;
00079 sample_params_.clear();
00080
00081 double min_vel_x = limits->min_vel_x;
00082 double max_vel_x = limits->max_vel_x;
00083 double min_vel_y = limits->min_vel_y;
00084 double max_vel_y = limits->max_vel_y;
00085
00086
00087 if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
00088
00089 Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
00090 Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
00091
00092 if ( ! use_dwa_) {
00093
00094
00095 double dist =
00096 sqrt((goal[0] - pos[0]) * (goal[0] - pos[0])) +
00097 (goal[1] - pos[1]) * (goal[1] - pos[1]);
00098
00099 max_vel_x = std::max(std::min(max_vel_x, 2 * dist / sim_time_), min_vel_x);
00100 max_vel_y = std::max(std::min(max_vel_y, 2 * dist / sim_time_), min_vel_y);
00101
00102
00103 max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
00104 max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
00105 max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
00106
00107 min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
00108 min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
00109 min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
00110 } else {
00111
00112 max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
00113 max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
00114 max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
00115
00116 min_vel[0] = std::max(limits->min_vel_x, vel[0] - acc_lim[0] * sim_period_);
00117 min_vel[1] = std::max(limits->min_vel_y, vel[1] - acc_lim[1] * sim_period_);
00118 min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
00119 }
00120
00121 Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
00122 VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
00123 VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
00124 VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
00125 for(; !x_it.isFinished(); x_it++) {
00126 vel_samp[0] = x_it.getVelocity();
00127 for(; !y_it.isFinished(); y_it++) {
00128 vel_samp[1] = y_it.getVelocity();
00129 for(; !th_it.isFinished(); th_it++) {
00130 vel_samp[2] = th_it.getVelocity();
00131
00132 sample_params_.push_back(vel_samp);
00133 }
00134 th_it.reset();
00135 }
00136 y_it.reset();
00137 }
00138 }
00139 }
00140
00141 void SimpleTrajectoryGenerator::setParameters(
00142 double sim_time,
00143 double sim_granularity,
00144 double angular_sim_granularity,
00145 bool use_dwa,
00146 double sim_period) {
00147 sim_time_ = sim_time;
00148 sim_granularity_ = sim_granularity;
00149 angular_sim_granularity_ = angular_sim_granularity;
00150 use_dwa_ = use_dwa;
00151 continued_acceleration_ = ! use_dwa_;
00152 sim_period_ = sim_period;
00153 }
00154
00158 bool SimpleTrajectoryGenerator::hasMoreTrajectories() {
00159 return next_sample_index_ < sample_params_.size();
00160 }
00161
00165 bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {
00166 bool result = false;
00167 if (hasMoreTrajectories()) {
00168 if (generateTrajectory(
00169 pos_,
00170 vel_,
00171 sample_params_[next_sample_index_],
00172 comp_traj)) {
00173 result = true;
00174 }
00175 }
00176 next_sample_index_++;
00177 return result;
00178 }
00179
00184 bool SimpleTrajectoryGenerator::generateTrajectory(
00185 Eigen::Vector3f pos,
00186 Eigen::Vector3f vel,
00187 Eigen::Vector3f sample_target_vel,
00188 base_local_planner::Trajectory& traj) {
00189 double vmag = sqrt(sample_target_vel[0] * sample_target_vel[0] + sample_target_vel[1] * sample_target_vel[1]);
00190 double eps = 1e-4;
00191 traj.cost_ = -1.0;
00192
00193 traj.resetPoints();
00194
00195
00196
00197 if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
00198 (limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
00199 return false;
00200 }
00201
00202 if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
00203 return false;
00204 }
00205
00206 int num_steps;
00207 if (discretize_by_time_) {
00208 num_steps = ceil(sim_time_ / sim_granularity_);
00209 } else {
00210
00211 double sim_time_distance = vmag * sim_time_;
00212 double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_;
00213 num_steps =
00214 ceil(std::max(sim_time_distance / sim_granularity_,
00215 sim_time_angle / angular_sim_granularity_));
00216 }
00217
00218
00219 double dt = sim_time_ / num_steps;
00220 traj.time_delta_ = dt;
00221
00222 Eigen::Vector3f loop_vel;
00223 if (continued_acceleration_) {
00224
00225 loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
00226 traj.xv_ = loop_vel[0];
00227 traj.yv_ = loop_vel[1];
00228 traj.thetav_ = loop_vel[2];
00229 } else {
00230
00231 loop_vel = sample_target_vel;
00232 traj.xv_ = sample_target_vel[0];
00233 traj.yv_ = sample_target_vel[1];
00234 traj.thetav_ = sample_target_vel[2];
00235 }
00236
00237
00238 for (int i = 0; i < num_steps; ++i) {
00239
00240
00241 traj.addPoint(pos[0], pos[1], pos[2]);
00242
00243 if (continued_acceleration_) {
00244
00245 loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
00246
00247 }
00248
00249
00250 pos = computeNewPositions(pos, loop_vel, dt);
00251
00252 }
00253
00254 return num_steps > 0;
00255 }
00256
00257 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f& pos,
00258 const Eigen::Vector3f& vel, double dt) {
00259 Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
00260 new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
00261 new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
00262 new_pos[2] = pos[2] + vel[2] * dt;
00263 return new_pos;
00264 }
00265
00269 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
00270 const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt) {
00271 Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
00272 for (int i = 0; i < 3; ++i) {
00273 if (vel[i] < sample_target_vel[i]) {
00274 new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
00275 } else {
00276 new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
00277 }
00278 }
00279 return new_vel;
00280 }
00281
00282 }