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
00039
00040
00041
00042
00043
00044
00045
00046
00047 #ifndef __dwa_local_planner__DWAPLANNERCONFIG_H__
00048 #define __dwa_local_planner__DWAPLANNERCONFIG_H__
00049
00050 #include <dynamic_reconfigure/config_tools.h>
00051 #include <limits>
00052 #include <ros/node_handle.h>
00053 #include <dynamic_reconfigure/ConfigDescription.h>
00054 #include <dynamic_reconfigure/ParamDescription.h>
00055 #include <dynamic_reconfigure/config_init_mutex.h>
00056
00057 namespace dwa_local_planner
00058 {
00059 class DWAPlannerConfigStatics;
00060
00061 class DWAPlannerConfig
00062 {
00063 public:
00064 class AbstractParamDescription : public dynamic_reconfigure::ParamDescription
00065 {
00066 public:
00067 AbstractParamDescription(std::string n, std::string t, uint32_t l,
00068 std::string d, std::string e)
00069 {
00070 name = n;
00071 type = t;
00072 level = l;
00073 description = d;
00074 edit_method = e;
00075 }
00076
00077 virtual void clamp(DWAPlannerConfig &config, const DWAPlannerConfig &max, const DWAPlannerConfig &min) const = 0;
00078 virtual void calcLevel(uint32_t &level, const DWAPlannerConfig &config1, const DWAPlannerConfig &config2) const = 0;
00079 virtual void fromServer(const ros::NodeHandle &nh, DWAPlannerConfig &config) const = 0;
00080 virtual void toServer(const ros::NodeHandle &nh, const DWAPlannerConfig &config) const = 0;
00081 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, DWAPlannerConfig &config) const = 0;
00082 virtual void toMessage(dynamic_reconfigure::Config &msg, const DWAPlannerConfig &config) const = 0;
00083 };
00084
00085 typedef boost::shared_ptr<AbstractParamDescription> AbstractParamDescriptionPtr;
00086 typedef boost::shared_ptr<const AbstractParamDescription> AbstractParamDescriptionConstPtr;
00087
00088 template <class T>
00089 class ParamDescription : public AbstractParamDescription
00090 {
00091 public:
00092 ParamDescription(std::string name, std::string type, uint32_t level,
00093 std::string description, std::string edit_method, T DWAPlannerConfig::* f) :
00094 AbstractParamDescription(name, type, level, description, edit_method),
00095 field(f)
00096 {}
00097
00098 T (DWAPlannerConfig::* field);
00099
00100 virtual void clamp(DWAPlannerConfig &config, const DWAPlannerConfig &max, const DWAPlannerConfig &min) const
00101 {
00102 if (config.*field > max.*field)
00103 config.*field = max.*field;
00104
00105 if (config.*field < min.*field)
00106 config.*field = min.*field;
00107 }
00108
00109 virtual void calcLevel(uint32_t &comb_level, const DWAPlannerConfig &config1, const DWAPlannerConfig &config2) const
00110 {
00111 if (config1.*field != config2.*field)
00112 comb_level |= level;
00113 }
00114
00115 virtual void fromServer(const ros::NodeHandle &nh, DWAPlannerConfig &config) const
00116 {
00117 nh.getParam(name, config.*field);
00118 }
00119
00120 virtual void toServer(const ros::NodeHandle &nh, const DWAPlannerConfig &config) const
00121 {
00122 nh.setParam(name, config.*field);
00123 }
00124
00125 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, DWAPlannerConfig &config) const
00126 {
00127 return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field);
00128 }
00129
00130 virtual void toMessage(dynamic_reconfigure::Config &msg, const DWAPlannerConfig &config) const
00131 {
00132 dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field);
00133 }
00134 };
00135
00136
00137 double max_trans_vel;
00138
00139 double min_trans_vel;
00140
00141 double max_vel_x;
00142
00143 double min_vel_x;
00144
00145 double max_vel_y;
00146
00147 double min_vel_y;
00148
00149 double max_rot_vel;
00150
00151 double min_rot_vel;
00152
00153 double sim_time;
00154
00155 double sim_granularity;
00156
00157 double path_distance_bias;
00158
00159 double goal_distance_bias;
00160
00161 double occdist_scale;
00162
00163 double stop_time_buffer;
00164
00165 double oscillation_reset_dist;
00166
00167 double forward_point_distance;
00168
00169 double scaling_speed;
00170
00171 double max_scaling_factor;
00172
00173 int vx_samples;
00174
00175 int vy_samples;
00176
00177 int vth_samples;
00178
00179 bool penalize_negative_x;
00180
00181
00182 bool __fromMessage__(dynamic_reconfigure::Config &msg)
00183 {
00184 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00185 int count = 0;
00186 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00187 if ((*i)->fromMessage(msg, *this))
00188 count++;
00189 if (count != dynamic_reconfigure::ConfigTools::size(msg))
00190 {
00191 ROS_ERROR("DWAPlannerConfig::__fromMessage__ called with an unexpected parameter.");
00192 ROS_ERROR("Booleans:");
00193 for (unsigned int i = 0; i < msg.bools.size(); i++)
00194 ROS_ERROR(" %s", msg.bools[i].name.c_str());
00195 ROS_ERROR("Integers:");
00196 for (unsigned int i = 0; i < msg.ints.size(); i++)
00197 ROS_ERROR(" %s", msg.ints[i].name.c_str());
00198 ROS_ERROR("Doubles:");
00199 for (unsigned int i = 0; i < msg.doubles.size(); i++)
00200 ROS_ERROR(" %s", msg.doubles[i].name.c_str());
00201 ROS_ERROR("Strings:");
00202 for (unsigned int i = 0; i < msg.strs.size(); i++)
00203 ROS_ERROR(" %s", msg.strs[i].name.c_str());
00204
00205
00206 return false;
00207 }
00208 return true;
00209 }
00210
00211
00212
00213 void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__) const
00214 {
00215 dynamic_reconfigure::ConfigTools::clear(msg);
00216 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00217 (*i)->toMessage(msg, *this);
00218 }
00219
00220 void __toMessage__(dynamic_reconfigure::Config &msg) const
00221 {
00222 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00223 __toMessage__(msg, __param_descriptions__);
00224 }
00225
00226 void __toServer__(const ros::NodeHandle &nh) const
00227 {
00228 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00229 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00230 (*i)->toServer(nh, *this);
00231 }
00232
00233 void __fromServer__(const ros::NodeHandle &nh)
00234 {
00235 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00236 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00237 (*i)->fromServer(nh, *this);
00238 }
00239
00240 void __clamp__()
00241 {
00242 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00243 const DWAPlannerConfig &__max__ = __getMax__();
00244 const DWAPlannerConfig &__min__ = __getMin__();
00245 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00246 (*i)->clamp(*this, __max__, __min__);
00247 }
00248
00249 uint32_t __level__(const DWAPlannerConfig &config) const
00250 {
00251 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00252 uint32_t level = 0;
00253 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00254 (*i)->calcLevel(level, config, *this);
00255 return level;
00256 }
00257
00258 static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__();
00259 static const DWAPlannerConfig &__getDefault__();
00260 static const DWAPlannerConfig &__getMax__();
00261 static const DWAPlannerConfig &__getMin__();
00262 static const std::vector<AbstractParamDescriptionConstPtr> &__getParamDescriptions__();
00263
00264 private:
00265 static const DWAPlannerConfigStatics *__get_statics__();
00266 };
00267
00268 template <>
00269 inline void DWAPlannerConfig::ParamDescription<std::string>::clamp(DWAPlannerConfig &config, const DWAPlannerConfig &max, const DWAPlannerConfig &min) const
00270 {
00271 return;
00272 }
00273
00274 class DWAPlannerConfigStatics
00275 {
00276 friend class DWAPlannerConfig;
00277
00278 DWAPlannerConfigStatics()
00279 {
00280
00281 __min__.max_trans_vel = 0.0;
00282
00283 __max__.max_trans_vel = std::numeric_limits<double>::infinity();
00284
00285 __default__.max_trans_vel = 0.55;
00286
00287 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("max_trans_vel", "double", 0, "The absolute value of the maximum translational velocity for the robot in m/s", "", &DWAPlannerConfig::max_trans_vel)));
00288
00289 __min__.min_trans_vel = 0.0;
00290
00291 __max__.min_trans_vel = std::numeric_limits<double>::infinity();
00292
00293 __default__.min_trans_vel = 0.1;
00294
00295 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("min_trans_vel", "double", 0, "The absolute value of the minimum translational velocity for the robot in m/s", "", &DWAPlannerConfig::min_trans_vel)));
00296
00297 __min__.max_vel_x = -std::numeric_limits<double>::infinity();
00298
00299 __max__.max_vel_x = std::numeric_limits<double>::infinity();
00300
00301 __default__.max_vel_x = 0.55;
00302
00303 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("max_vel_x", "double", 0, "The maximum x velocity for the robot in m/s", "", &DWAPlannerConfig::max_vel_x)));
00304
00305 __min__.min_vel_x = -std::numeric_limits<double>::infinity();
00306
00307 __max__.min_vel_x = std::numeric_limits<double>::infinity();
00308
00309 __default__.min_vel_x = 0.0;
00310
00311 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("min_vel_x", "double", 0, "The minimum x velocity for the robot in m/s", "", &DWAPlannerConfig::min_vel_x)));
00312
00313 __min__.max_vel_y = -std::numeric_limits<double>::infinity();
00314
00315 __max__.max_vel_y = std::numeric_limits<double>::infinity();
00316
00317 __default__.max_vel_y = 0.1;
00318
00319 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("max_vel_y", "double", 0, "The maximum y velocity for the robot in m/s", "", &DWAPlannerConfig::max_vel_y)));
00320
00321 __min__.min_vel_y = -std::numeric_limits<double>::infinity();
00322
00323 __max__.min_vel_y = std::numeric_limits<double>::infinity();
00324
00325 __default__.min_vel_y = -0.1;
00326
00327 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("min_vel_y", "double", 0, "The minimum y velocity for the robot in m/s", "", &DWAPlannerConfig::min_vel_y)));
00328
00329 __min__.max_rot_vel = 0.0;
00330
00331 __max__.max_rot_vel = std::numeric_limits<double>::infinity();
00332
00333 __default__.max_rot_vel = 1.0;
00334
00335 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("max_rot_vel", "double", 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", "", &DWAPlannerConfig::max_rot_vel)));
00336
00337 __min__.min_rot_vel = 0.0;
00338
00339 __max__.min_rot_vel = std::numeric_limits<double>::infinity();
00340
00341 __default__.min_rot_vel = 0.4;
00342
00343 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("min_rot_vel", "double", 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", "", &DWAPlannerConfig::min_rot_vel)));
00344
00345 __min__.sim_time = 0.0;
00346
00347 __max__.sim_time = std::numeric_limits<double>::infinity();
00348
00349 __default__.sim_time = 1.7;
00350
00351 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("sim_time", "double", 0, "The amount of time to roll trajectories out for in seconds", "", &DWAPlannerConfig::sim_time)));
00352
00353 __min__.sim_granularity = 0.0;
00354
00355 __max__.sim_granularity = std::numeric_limits<double>::infinity();
00356
00357 __default__.sim_granularity = 0.025;
00358
00359 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("sim_granularity", "double", 0, "The granularity with which to check for collisions along each trajectory in meters", "", &DWAPlannerConfig::sim_granularity)));
00360
00361 __min__.path_distance_bias = -std::numeric_limits<double>::infinity();
00362
00363 __max__.path_distance_bias = std::numeric_limits<double>::infinity();
00364
00365 __default__.path_distance_bias = 32.0;
00366
00367 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("path_distance_bias", "double", 0, "The weight for the path distance part of the cost function", "", &DWAPlannerConfig::path_distance_bias)));
00368
00369 __min__.goal_distance_bias = -std::numeric_limits<double>::infinity();
00370
00371 __max__.goal_distance_bias = std::numeric_limits<double>::infinity();
00372
00373 __default__.goal_distance_bias = 24.0;
00374
00375 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("goal_distance_bias", "double", 0, "The weight for the goal distance part of the cost function", "", &DWAPlannerConfig::goal_distance_bias)));
00376
00377 __min__.occdist_scale = -std::numeric_limits<double>::infinity();
00378
00379 __max__.occdist_scale = std::numeric_limits<double>::infinity();
00380
00381 __default__.occdist_scale = 0.01;
00382
00383 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("occdist_scale", "double", 0, "The weight for the obstacle distance part of the cost function", "", &DWAPlannerConfig::occdist_scale)));
00384
00385 __min__.stop_time_buffer = 0.0;
00386
00387 __max__.stop_time_buffer = std::numeric_limits<double>::infinity();
00388
00389 __default__.stop_time_buffer = 0.2;
00390
00391 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("stop_time_buffer", "double", 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", "", &DWAPlannerConfig::stop_time_buffer)));
00392
00393 __min__.oscillation_reset_dist = 0.0;
00394
00395 __max__.oscillation_reset_dist = std::numeric_limits<double>::infinity();
00396
00397 __default__.oscillation_reset_dist = 0.05;
00398
00399 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("oscillation_reset_dist", "double", 0, "The distance the robot must travel before oscillation flags are reset, in meters", "", &DWAPlannerConfig::oscillation_reset_dist)));
00400
00401 __min__.forward_point_distance = -std::numeric_limits<double>::infinity();
00402
00403 __max__.forward_point_distance = std::numeric_limits<double>::infinity();
00404
00405 __default__.forward_point_distance = 0.325;
00406
00407 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("forward_point_distance", "double", 0, "The distance from the center point of the robot to place an additional scoring point, in meters", "", &DWAPlannerConfig::forward_point_distance)));
00408
00409 __min__.scaling_speed = 0.0;
00410
00411 __max__.scaling_speed = std::numeric_limits<double>::infinity();
00412
00413 __default__.scaling_speed = 0.25;
00414
00415 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("scaling_speed", "double", 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", "", &DWAPlannerConfig::scaling_speed)));
00416
00417 __min__.max_scaling_factor = 0.0;
00418
00419 __max__.max_scaling_factor = std::numeric_limits<double>::infinity();
00420
00421 __default__.max_scaling_factor = 0.2;
00422
00423 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<double>("max_scaling_factor", "double", 0, "The maximum factor to scale the robot's footprint by", "", &DWAPlannerConfig::max_scaling_factor)));
00424
00425 __min__.vx_samples = 1;
00426
00427 __max__.vx_samples = 2147483647;
00428
00429 __default__.vx_samples = 3;
00430
00431 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<int>("vx_samples", "int", 0, "The number of samples to use when exploring the x velocity space", "", &DWAPlannerConfig::vx_samples)));
00432
00433 __min__.vy_samples = 1;
00434
00435 __max__.vy_samples = 2147483647;
00436
00437 __default__.vy_samples = 10;
00438
00439 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<int>("vy_samples", "int", 0, "The number of samples to use when exploring the y velocity space", "", &DWAPlannerConfig::vy_samples)));
00440
00441 __min__.vth_samples = 1;
00442
00443 __max__.vth_samples = 2147483647;
00444
00445 __default__.vth_samples = 20;
00446
00447 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<int>("vth_samples", "int", 0, "The number of samples to use when exploring the theta velocity space", "", &DWAPlannerConfig::vth_samples)));
00448
00449 __min__.penalize_negative_x = 0;
00450
00451 __max__.penalize_negative_x = 1;
00452
00453 __default__.penalize_negative_x = 1;
00454
00455 __param_descriptions__.push_back(DWAPlannerConfig::AbstractParamDescriptionConstPtr(new DWAPlannerConfig::ParamDescription<bool>("penalize_negative_x", "bool", 0, "Whether to penalize trajectories that have negative x velocities.", "", &DWAPlannerConfig::penalize_negative_x)));
00456
00457
00458 for (std::vector<DWAPlannerConfig::AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00459 __description_message__.parameters.push_back(**i);
00460 __max__.__toMessage__(__description_message__.max, __param_descriptions__);
00461 __min__.__toMessage__(__description_message__.min, __param_descriptions__);
00462 __default__.__toMessage__(__description_message__.dflt, __param_descriptions__);
00463 }
00464 std::vector<DWAPlannerConfig::AbstractParamDescriptionConstPtr> __param_descriptions__;
00465 DWAPlannerConfig __max__;
00466 DWAPlannerConfig __min__;
00467 DWAPlannerConfig __default__;
00468 dynamic_reconfigure::ConfigDescription __description_message__;
00469 static const DWAPlannerConfigStatics *get_instance()
00470 {
00471
00472
00473
00474
00475 static DWAPlannerConfigStatics instance;
00476 return &instance;
00477 }
00478 };
00479
00480 inline const dynamic_reconfigure::ConfigDescription &DWAPlannerConfig::__getDescriptionMessage__()
00481 {
00482 return __get_statics__()->__description_message__;
00483 }
00484
00485 inline const DWAPlannerConfig &DWAPlannerConfig::__getDefault__()
00486 {
00487 return __get_statics__()->__default__;
00488 }
00489
00490 inline const DWAPlannerConfig &DWAPlannerConfig::__getMax__()
00491 {
00492 return __get_statics__()->__max__;
00493 }
00494
00495 inline const DWAPlannerConfig &DWAPlannerConfig::__getMin__()
00496 {
00497 return __get_statics__()->__min__;
00498 }
00499
00500 inline const std::vector<DWAPlannerConfig::AbstractParamDescriptionConstPtr> &DWAPlannerConfig::__getParamDescriptions__()
00501 {
00502 return __get_statics__()->__param_descriptions__;
00503 }
00504
00505 inline const DWAPlannerConfigStatics *DWAPlannerConfig::__get_statics__()
00506 {
00507 const static DWAPlannerConfigStatics *statics;
00508
00509 if (statics)
00510 return statics;
00511
00512 boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
00513
00514 if (statics)
00515 return statics;
00516
00517 statics = DWAPlannerConfigStatics::get_instance();
00518
00519 return statics;
00520 }
00521
00522
00523 }
00524
00525 #endif // __DWAPLANNERRECONFIGURATOR_H__
