diff --git a/nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h b/nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h index 733f575ed6b80174c470f92b51b1e983c983032d..79a713a99ce91fbcb63ce577ae96caf7e427e3d7 100644 --- a/nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h +++ b/nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h @@ -103,7 +103,7 @@ protected: // Cached square values of min_speed_xy and max_speed_xy double min_speed_xy_sq_, max_speed_xy_sq_; - // void reconfigureCB(KinematicParamsConfig &config, uint32_t level); + void reconfigureCB(); // std::shared_ptr<dynamic_reconfigure::Server<KinematicParamsConfig> > dsrv_; }; diff --git a/nav2_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_controller/dwb_plugins/src/kinematic_parameters.cpp index f3a062ebecde527eb571cbb3180bbc21bfc9249b..5dd76cd7500d3291453fe8504f422de818297110 100644 --- a/nav2_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -85,35 +85,47 @@ void KinematicParameters::initialize(const std::shared_ptr<rclcpp::Node> & nh) setDecelerationAsNeeded(nh, "y"); setDecelerationAsNeeded(nh, "theta"); + // TODO(crdelsey): Handle dynamic params // the rest of the initial values are loaded through the dynamic reconfigure mechanisms // dsrv_ = std::make_shared<dynamic_reconfigure::Server<KinematicParamsConfig> >(nh); // dynamic_reconfigure::Server<KinematicParamsConfig>::CallbackType cb = // boost::bind(&KinematicParameters::reconfigureCB, this, _1, _2); - // dsrv_->setCallback(cb); + reconfigureCB(); } -// TODO(crdelsey): Handle params -// void KinematicParameters::reconfigureCB(KinematicParamsConfig &config, uint32_t level) -// { -// min_vel_x_ = config.min_vel_x; -// min_vel_y_ = config.min_vel_y; -// max_vel_x_ = config.max_vel_x; -// max_vel_y_ = config.max_vel_y; -// max_vel_theta_ = config.max_vel_theta; -// -// min_speed_xy_ = config.min_speed_xy; -// max_speed_xy_ = config.max_speed_xy; -// min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_; -// max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_; -// min_speed_theta_ = config.min_speed_theta; -// -// acc_lim_x_ = config.acc_lim_x; -// acc_lim_y_ = config.acc_lim_y; -// acc_lim_theta_ = config.acc_lim_theta; -// decel_lim_x_ = config.decel_lim_x; -// decel_lim_y_ = config.decel_lim_y; -// decel_lim_theta_ = config.decel_lim_theta; -// } +void KinematicParameters::reconfigureCB() +{ + // TODO(crdelsey): Remove hard coded parameters when dynamic_reconfigure is in + min_vel_x_ = -0.26; + max_vel_x_ = 0.26; + max_vel_theta_ = 1.0; + + max_speed_xy_ = max_vel_x_; + + acc_lim_x_ = 2.5; + acc_lim_theta_ = 3.2; + decel_lim_x_ = -acc_lim_x_; + decel_lim_theta_ = -acc_lim_theta_; + + // min_vel_x_ = config.min_vel_x; + // min_vel_y_ = config.min_vel_y; + // max_vel_x_ = config.max_vel_x; + // max_vel_y_ = config.max_vel_y; + // max_vel_theta_ = config.max_vel_theta; + // + // min_speed_xy_ = config.min_speed_xy; + // max_speed_xy_ = config.max_speed_xy; + min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_; + max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_; + // min_speed_theta_ = config.min_speed_theta; + // + // acc_lim_x_ = config.acc_lim_x; + // acc_lim_y_ = config.acc_lim_y; + // acc_lim_theta_ = config.acc_lim_theta; + // decel_lim_x_ = config.decel_lim_x; + // decel_lim_y_ = config.decel_lim_y; + // decel_lim_theta_ = config.decel_lim_theta; +} bool KinematicParameters::isValidSpeed(double x, double y, double theta) {