Go to the documentation of this file.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 #ifndef SIMPLE_TRAJECTORY_GENERATOR_H_
00039 #define SIMPLE_TRAJECTORY_GENERATOR_H_
00040
00041 #include <base_local_planner/trajectory_sample_generator.h>
00042 #include <base_local_planner/local_planner_limits.h>
00043 #include <Eigen/Core>
00044
00045 namespace base_local_planner {
00046
00061 class SimpleTrajectoryGenerator: public base_local_planner::TrajectorySampleGenerator {
00062 public:
00063
00064 SimpleTrajectoryGenerator() {
00065 limits_ = NULL;
00066 }
00067
00068 ~SimpleTrajectoryGenerator() {}
00069
00079 void initialise(
00080 const Eigen::Vector3f& pos,
00081 const Eigen::Vector3f& vel,
00082 const Eigen::Vector3f& goal,
00083 base_local_planner::LocalPlannerLimits* limits,
00084 const Eigen::Vector3f& vsamples,
00085 std::vector<Eigen::Vector3f> additional_samples,
00086 bool discretize_by_time = false);
00087
00096 void initialise(
00097 const Eigen::Vector3f& pos,
00098 const Eigen::Vector3f& vel,
00099 const Eigen::Vector3f& goal,
00100 base_local_planner::LocalPlannerLimits* limits,
00101 const Eigen::Vector3f& vsamples,
00102 bool discretize_by_time = false);
00103
00112 void setParameters(double sim_time,
00113 double sim_granularity,
00114 double angular_sim_granularity,
00115 bool use_dwa = false,
00116 double sim_period = 0.0);
00117
00121 bool hasMoreTrajectories();
00122
00126 bool nextTrajectory(Trajectory &traj);
00127
00128
00129 static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f& pos,
00130 const Eigen::Vector3f& vel, double dt);
00131
00132 static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
00133 const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt);
00134
00135 bool generateTrajectory(
00136 Eigen::Vector3f pos,
00137 Eigen::Vector3f vel,
00138 Eigen::Vector3f sample_target_vel,
00139 base_local_planner::Trajectory& traj);
00140
00141 protected:
00142
00143 unsigned int next_sample_index_;
00144
00145 std::vector<Eigen::Vector3f> sample_params_;
00146 base_local_planner::LocalPlannerLimits* limits_;
00147 Eigen::Vector3f pos_;
00148 Eigen::Vector3f vel_;
00149
00150
00151 bool continued_acceleration_;
00152 bool discretize_by_time_;
00153
00154 double sim_time_, sim_granularity_, angular_sim_granularity_;
00155 bool use_dwa_;
00156 double sim_period_;
00157 };
00158
00159 }
00160 #endif