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
00080 void initialise(
00081 const Eigen::Vector3f& pos,
00082 const Eigen::Vector3f& vel,
00083 const Eigen::Vector3f& goal,
00084 base_local_planner::LocalPlannerLimits* limits,
00085 const Eigen::Vector3f& vsamples,
00086 std::vector<Eigen::Vector3f> additional_samples,
00087 bool discretize_by_time = false);
00088
00098 void initialise(
00099 const Eigen::Vector3f& pos,
00100 const Eigen::Vector3f& vel,
00101 const Eigen::Vector3f& goal,
00102 base_local_planner::LocalPlannerLimits* limits,
00103 const Eigen::Vector3f& vsamples,
00104 bool discretize_by_time = false);
00105
00109 void setParameters(double sim_time,
00110 double sim_granularity,
00111 double angular_sim_granularity,
00112 bool use_dwa = false,
00113 double sim_period = 0.0);
00114
00118 bool hasMoreTrajectories();
00119
00123 bool nextTrajectory(Trajectory &traj);
00124
00125
00126 static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f& pos,
00127 const Eigen::Vector3f& vel, double dt);
00128
00129 static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
00130 const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt);
00131
00132 bool generateTrajectory(
00133 Eigen::Vector3f pos,
00134 Eigen::Vector3f vel,
00135 Eigen::Vector3f sample_target_vel,
00136 base_local_planner::Trajectory& traj);
00137
00138 protected:
00139
00140 unsigned int next_sample_index_;
00141
00142 std::vector<Eigen::Vector3f> sample_params_;
00143 base_local_planner::LocalPlannerLimits* limits_;
00144 Eigen::Vector3f pos_;
00145 Eigen::Vector3f vel_;
00146
00147
00148 bool continued_acceleration_;
00149 bool discretize_by_time_;
00150
00151 double sim_time_, sim_granularity_, angular_sim_granularity_;
00152 bool use_dwa_;
00153 double sim_period_;
00154 };
00155
00156 }
00157 #endif