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 #include <dwb_plugins/kinematic_parameters.h>
00036 #include <nav_2d_utils/parameters.h>
00037 #include <string>
00038
00039 using nav_2d_utils::moveDeprecatedParameter;
00040 namespace dwb_plugins
00041 {
00042
00043 const double EPSILON = 1E-5;
00044
00050 void setDecelerationAsNeeded(const ros::NodeHandle& nh, const std::string dimension)
00051 {
00052 std::string decel_param = "decel_lim_" + dimension;
00053 if (nh.hasParam(decel_param)) return;
00054
00055 std::string accel_param = "acc_lim_" + dimension;
00056 if (!nh.hasParam(accel_param)) return;
00057
00058 double accel;
00059 nh.getParam(accel_param, accel);
00060 nh.setParam(decel_param, -accel);
00061 }
00062
00063 KinematicParameters::KinematicParameters() :
00064 min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
00065 min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
00066 acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
00067 decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
00068 min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0),
00069 dsrv_(nullptr)
00070 {
00071 }
00072
00073 void KinematicParameters::initialize(const ros::NodeHandle& nh)
00074 {
00075
00076 moveDeprecatedParameter<double>(nh, "max_vel_theta", "max_rot_vel");
00077 moveDeprecatedParameter<double>(nh, "min_speed_xy", "min_trans_vel");
00078 moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
00079 moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");
00080
00081
00082 setDecelerationAsNeeded(nh, "x");
00083 setDecelerationAsNeeded(nh, "y");
00084 setDecelerationAsNeeded(nh, "theta");
00085
00086
00087 dsrv_ = std::make_shared<dynamic_reconfigure::Server<KinematicParamsConfig> >(nh);
00088 dynamic_reconfigure::Server<KinematicParamsConfig>::CallbackType cb =
00089 boost::bind(&KinematicParameters::reconfigureCB, this, _1, _2);
00090 dsrv_->setCallback(cb);
00091 }
00092
00093 void KinematicParameters::reconfigureCB(KinematicParamsConfig &config, uint32_t level)
00094 {
00095 min_vel_x_ = config.min_vel_x;
00096 min_vel_y_ = config.min_vel_y;
00097 max_vel_x_ = config.max_vel_x;
00098 max_vel_y_ = config.max_vel_y;
00099 max_vel_theta_ = config.max_vel_theta;
00100
00101 min_speed_xy_ = config.min_speed_xy;
00102 max_speed_xy_ = config.max_speed_xy;
00103 min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
00104 max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
00105 min_speed_theta_ = config.min_speed_theta;
00106
00107 acc_lim_x_ = config.acc_lim_x;
00108 acc_lim_y_ = config.acc_lim_y;
00109 acc_lim_theta_ = config.acc_lim_theta;
00110 decel_lim_x_ = config.decel_lim_x;
00111 decel_lim_y_ = config.decel_lim_y;
00112 decel_lim_theta_ = config.decel_lim_theta;
00113 }
00114
00115 bool KinematicParameters::isValidSpeed(double x, double y, double theta)
00116 {
00117 double vmag_sq = x * x + y * y;
00118 if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) return false;
00119 if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
00120 min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) return false;
00121 if (vmag_sq == 0.0 && theta == 0.0) return false;
00122 return true;
00123 }
00124
00125 }