Added thread synchronization to KinematicParameters (#1459)

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<KinematicParameters>;
-
 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<KinematicsHandler>;
+
+protected:
+  std::atomic<KinematicParameters *> kinematics_;
 
   // Subscription for parameter change
   rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::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_;
 };
 

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<double> getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel);
 
-  KinematicParameters::Ptr kinematics_;
+  KinematicsHandler::Ptr kinematics_handler_;
   std::shared_ptr<VelocityIterator> velocity_iterator_;
 
   double sim_time_;

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;

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<OneDVelocityIterator> x_it_, y_it_, th_it_;
 };

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<rclcpp::AsyncParametersClient>(
@@ -117,63 +135,71 @@ 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;
     const auto & value = changed_parameter.value;
 
     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

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<KinematicParameters>();
-  kinematics_->initialize(nh, plugin_name_);
+  kinematics_handler_ = std::make_shared<KinematicsHandler>();
+  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<XYThetaIterator>();
-  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;
 }

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<OneDVelocityIterator>(
     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<OneDVelocityIterator>(
     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<OneDVelocityIterator>(
     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());
 }