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