From 2f62e66702d9b7f8d819901e59fcbc1c5937e420 Mon Sep 17 00:00:00 2001 From: Aitor Miguel Blanco Date: Wed, 1 Apr 2020 11:32:31 +1100 Subject: [PATCH 1/2] Added thread synchronization to KinematicParameters (#1459) Signed-off-by: Aitor Miguel Blanco --- .../dwb_plugins/kinematic_parameters.hpp | 33 ++++-- .../dwb_plugins/standard_traj_generator.hpp | 2 +- .../include/dwb_plugins/velocity_iterator.hpp | 2 +- .../include/dwb_plugins/xy_theta_iterator.hpp | 6 +- .../dwb_plugins/src/kinematic_parameters.cpp | 108 +++++++++++------- .../src/standard_traj_generator.cpp | 17 +-- .../dwb_plugins/src/xy_theta_iterator.cpp | 19 +-- 7 files changed, 115 insertions(+), 72 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index 5ff9de0a28..4712d5ee67 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -48,12 +48,8 @@ namespace dwb_plugins * @class KinematicParameters * @brief A class containing one representation of the robot's kinematics */ -class KinematicParameters +struct KinematicParameters { -public: - KinematicParameters(); - void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name); - inline double getMinX() {return min_vel_x_;} inline double getMaxX() {return max_vel_x_;} inline double getAccX() {return acc_lim_x_;} @@ -90,8 +86,6 @@ class KinematicParameters */ bool isValidSpeed(double x, double y, double theta); - using Ptr = std::shared_ptr; - protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -113,13 +107,34 @@ class KinematicParameters double min_speed_xy_sq_{0}; double max_speed_xy_sq_{0}; - void reconfigureCB(); + friend class KinematicsHandler; +}; + +/** + * @class KinematicsHandler + * @brief A class managing the representation of the robot's kinematics + */ +class KinematicsHandler +{ +public: + KinematicsHandler(); + ~KinematicsHandler(); + void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name); + + inline KinematicParameters getKinematics() {return *kinematics_.load();} + + bool isValidSpeed(double x, double y, double theta); + + using Ptr = std::shared_ptr; + +protected: + std::atomic kinematics_; // Subscription for parameter change rclcpp::AsyncParametersClient::SharedPtr parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); - + void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index a308e3543d..ec380674d4 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -113,7 +113,7 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator */ virtual std::vector getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel); - KinematicParameters::Ptr kinematics_; + KinematicsHandler::Ptr kinematics_handler_; std::shared_ptr velocity_iterator_; double sim_time_; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp index e0acbfb2a5..55a663fddb 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp @@ -51,7 +51,7 @@ class VelocityIterator virtual ~VelocityIterator() {} virtual void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) = 0; virtual void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) = 0; virtual bool hasMoreTwists() = 0; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 3b448479e8..68ac9405c2 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -48,10 +48,10 @@ class XYThetaIterator : public VelocityIterator { public: XYThetaIterator() - : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} + : kinematics_handler_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) override; void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) override; bool hasMoreTwists() override; @@ -61,7 +61,7 @@ class XYThetaIterator : public VelocityIterator virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; - KinematicParameters::Ptr kinematics_; + KinematicsHandler::Ptr kinematics_handler_; std::shared_ptr x_it_, y_it_, th_it_; }; diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index 74c415e7eb..4b4747cd67 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -52,11 +52,27 @@ using std::placeholders::_1; namespace dwb_plugins { -KinematicParameters::KinematicParameters() +bool KinematicParameters::isValidSpeed(double x, double y, double theta) +{ + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;} + if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;} + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; +} + +KinematicsHandler::KinematicsHandler() +{ + kinematics_.store(new KinematicParameters); +} + +KinematicsHandler::~KinematicsHandler() { + delete kinematics_.load(); } -void KinematicParameters::initialize( +void KinematicsHandler::initialize( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name) { @@ -94,20 +110,22 @@ void KinematicParameters::initialize( nh, plugin_name + ".decel_lim_theta", rclcpp::ParameterValue(0.0)); - nh->get_parameter(plugin_name + ".min_vel_x", min_vel_x_); - nh->get_parameter(plugin_name + ".min_vel_y", min_vel_y_); - nh->get_parameter(plugin_name + ".max_vel_x", max_vel_x_); - nh->get_parameter(plugin_name + ".max_vel_y", max_vel_y_); - nh->get_parameter(plugin_name + ".max_vel_theta", max_vel_theta_); - nh->get_parameter(plugin_name + ".min_speed_xy", min_speed_xy_); - nh->get_parameter(plugin_name + ".max_speed_xy", max_speed_xy_); - nh->get_parameter(plugin_name + ".min_speed_theta", min_speed_theta_); - nh->get_parameter(plugin_name + ".acc_lim_x", acc_lim_x_); - nh->get_parameter(plugin_name + ".acc_lim_y", acc_lim_y_); - nh->get_parameter(plugin_name + ".acc_lim_theta", acc_lim_theta_); - nh->get_parameter(plugin_name + ".decel_lim_x", decel_lim_x_); - nh->get_parameter(plugin_name + ".decel_lim_y", decel_lim_y_); - nh->get_parameter(plugin_name + ".decel_lim_theta", decel_lim_theta_); + KinematicParameters kinematics; + + nh->get_parameter(plugin_name + ".min_vel_x", kinematics.min_vel_x_); + nh->get_parameter(plugin_name + ".min_vel_y", kinematics.min_vel_y_); + nh->get_parameter(plugin_name + ".max_vel_x", kinematics.max_vel_x_); + nh->get_parameter(plugin_name + ".max_vel_y", kinematics.max_vel_y_); + nh->get_parameter(plugin_name + ".max_vel_theta", kinematics.max_vel_theta_); + nh->get_parameter(plugin_name + ".min_speed_xy", kinematics.min_speed_xy_); + nh->get_parameter(plugin_name + ".max_speed_xy", kinematics.max_speed_xy_); + nh->get_parameter(plugin_name + ".min_speed_theta", kinematics.min_speed_theta_); + nh->get_parameter(plugin_name + ".acc_lim_x", kinematics.acc_lim_x_); + nh->get_parameter(plugin_name + ".acc_lim_y", kinematics.acc_lim_y_); + nh->get_parameter(plugin_name + ".acc_lim_theta", kinematics.acc_lim_theta_); + nh->get_parameter(plugin_name + ".decel_lim_x", kinematics.decel_lim_x_); + nh->get_parameter(plugin_name + ".decel_lim_y", kinematics.decel_lim_y_); + nh->get_parameter(plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_); // Setup callback for changes to parameters. parameters_client_ = std::make_shared( @@ -117,26 +135,25 @@ void KinematicParameters::initialize( nh->get_node_services_interface()); parameter_event_sub_ = parameters_client_->on_parameter_event( - std::bind(&KinematicParameters::on_parameter_event_callback, this, _1)); + std::bind(&KinematicsHandler::on_parameter_event_callback, this, _1)); - min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_; - max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_; + kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; + kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; + + update_kinematics(kinematics); } -bool KinematicParameters::isValidSpeed(double x, double y, double theta) +bool KinematicsHandler::isValidSpeed(double x, double y, double theta) { - double vmag_sq = x * x + y * y; - if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;} - if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ && - min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;} - if (vmag_sq == 0.0 && theta == 0.0) {return false;} - return true; + return kinematics_.load()->isValidSpeed(x, y, theta); } void -KinematicParameters::on_parameter_event_callback( +KinematicsHandler::on_parameter_event_callback( const rcl_interfaces::msg::ParameterEvent::SharedPtr event) { + KinematicParameters kinematics(*kinematics_.load()); + for (auto & changed_parameter : event->changed_parameters) { const auto & type = changed_parameter.value.type; const auto & name = changed_parameter.name; @@ -144,36 +161,45 @@ KinematicParameters::on_parameter_event_callback( if (type == ParameterType::PARAMETER_DOUBLE) { if (name == plugin_name_ + ".min_vel_x") { - min_vel_x_ = value.double_value; + kinematics.min_vel_x_ = value.double_value; } else if (name == plugin_name_ + ".min_vel_y") { - min_vel_y_ = value.double_value; + kinematics.min_vel_y_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_x") { - max_vel_x_ = value.double_value; + kinematics.max_vel_x_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_y") { - max_vel_y_ = value.double_value; + kinematics.max_vel_y_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_theta") { - max_vel_theta_ = value.double_value; + kinematics.max_vel_theta_ = value.double_value; } else if (name == plugin_name_ + ".min_speed_xy") { - min_speed_xy_ = value.double_value; + kinematics.min_speed_xy_ = value.double_value; + kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; } else if (name == plugin_name_ + ".max_speed_xy") { - max_speed_xy_ = value.double_value; + kinematics.max_speed_xy_ = value.double_value; } else if (name == plugin_name_ + ".min_speed_theta") { - min_speed_theta_ = value.double_value; + kinematics.min_speed_theta_ = value.double_value; + kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; } else if (name == plugin_name_ + ".acc_lim_x") { - acc_lim_x_ = value.double_value; + kinematics.acc_lim_x_ = value.double_value; } else if (name == plugin_name_ + ".acc_lim_y") { - acc_lim_y_ = value.double_value; + kinematics.acc_lim_y_ = value.double_value; } else if (name == plugin_name_ + ".acc_lim_theta") { - acc_lim_theta_ = value.double_value; + kinematics.acc_lim_theta_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_x") { - decel_lim_x_ = value.double_value; + kinematics.decel_lim_x_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_y") { - decel_lim_y_ = value.double_value; + kinematics.decel_lim_y_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_theta") { - decel_lim_theta_ = value.double_value; + kinematics.decel_lim_theta_ = value.double_value; } } } + update_kinematics(kinematics); +} + +void KinematicsHandler::update_kinematics(KinematicParameters kinematics) +{ + delete kinematics_.load(); + kinematics_.store(new KinematicParameters(kinematics)); } } // namespace dwb_plugins diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 8c2dd783e0..3b7e168579 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -53,8 +53,8 @@ void StandardTrajectoryGenerator::initialize( const std::string & plugin_name) { plugin_name_ = plugin_name; - kinematics_ = std::make_shared(); - kinematics_->initialize(nh, plugin_name_); + kinematics_handler_ = std::make_shared(); + kinematics_handler_->initialize(nh, plugin_name_); initializeIterator(nh); nav2_util::declare_parameter_if_not_declared( @@ -97,7 +97,7 @@ void StandardTrajectoryGenerator::initializeIterator( const nav2_util::LifecycleNode::SharedPtr & nh) { velocity_iterator_ = std::make_shared(); - velocity_iterator_->initialize(nh, kinematics_, plugin_name_); + velocity_iterator_->initialize(nh, kinematics_handler_, plugin_name_); } void StandardTrajectoryGenerator::startNewIteration( @@ -185,16 +185,17 @@ nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity( const nav_2d_msgs::msg::Twist2D & cmd_vel, const nav_2d_msgs::msg::Twist2D & start_vel, const double dt) { + KinematicParameters kinematics = kinematics_handler_->getKinematics(); nav_2d_msgs::msg::Twist2D new_vel; new_vel.x = projectVelocity( - start_vel.x, kinematics_->getAccX(), - kinematics_->getDecelX(), dt, cmd_vel.x); + start_vel.x, kinematics.getAccX(), + kinematics.getDecelX(), dt, cmd_vel.x); new_vel.y = projectVelocity( - start_vel.y, kinematics_->getAccY(), - kinematics_->getDecelY(), dt, cmd_vel.y); + start_vel.y, kinematics.getAccY(), + kinematics.getDecelY(), dt, cmd_vel.y); new_vel.theta = projectVelocity( start_vel.theta, - kinematics_->getAccTheta(), kinematics_->getDecelTheta(), + kinematics.getAccTheta(), kinematics.getDecelTheta(), dt, cmd_vel.theta); return new_vel; } diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 873f303629..0474e167be 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -42,10 +42,10 @@ namespace dwb_plugins { void XYThetaIterator::initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) { - kinematics_ = kinematics; + kinematics_handler_ = kinematics; nav2_util::declare_parameter_if_not_declared( nh, @@ -66,18 +66,19 @@ void XYThetaIterator::startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) { + KinematicParameters kinematics = kinematics_handler_->getKinematics(); x_it_ = std::make_shared( current_velocity.x, - kinematics_->getMinX(), kinematics_->getMaxX(), - kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); + kinematics.getMinX(), kinematics.getMaxX(), + kinematics.getAccX(), kinematics.getDecelX(), dt, vx_samples_); y_it_ = std::make_shared( current_velocity.y, - kinematics_->getMinY(), kinematics_->getMaxY(), - kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_); + kinematics.getMinY(), kinematics.getMaxY(), + kinematics.getAccY(), kinematics.getDecelY(), dt, vy_samples_); th_it_ = std::make_shared( current_velocity.theta, - kinematics_->getMinTheta(), kinematics_->getMaxTheta(), - kinematics_->getAccTheta(), kinematics_->getDecelTheta(), + kinematics.getMinTheta(), kinematics.getMaxTheta(), + kinematics.getAccTheta(), kinematics.getDecelTheta(), dt, vtheta_samples_); if (!isValidVelocity()) { iterateToValidVelocity(); @@ -86,7 +87,7 @@ void XYThetaIterator::startNewIteration( bool XYThetaIterator::isValidVelocity() { - return kinematics_->isValidSpeed( + return kinematics_handler_->isValidSpeed( x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } From 554887d26513ae6c1b4f0c4703f66d7edd367485 Mon Sep 17 00:00:00 2001 From: Aitor Miguel Blanco Date: Thu, 2 Apr 2020 12:30:48 +1100 Subject: [PATCH 2/2] Doxygen fix (#1459) Signed-off-by: Aitor Miguel Blanco --- .../include/dwb_plugins/kinematic_parameters.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index 4712d5ee67..577657b9a0 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -45,11 +45,13 @@ namespace dwb_plugins { /** - * @class KinematicParameters - * @brief A class containing one representation of the robot's kinematics + * @struct KinematicParameters + * @brief A struct containing one representation of the robot's kinematics */ struct KinematicParameters { + friend class KinematicsHandler; + inline double getMinX() {return min_vel_x_;} inline double getMaxX() {return max_vel_x_;} inline double getAccX() {return acc_lim_x_;} @@ -106,8 +108,6 @@ struct KinematicParameters // Cached square values of min_speed_xy and max_speed_xy double min_speed_xy_sq_{0}; double max_speed_xy_sq_{0}; - - friend class KinematicsHandler; }; /**