Merge branch 'master' into add/statistics/hardware_components

Author: bmagyar

doc/introspection.rst

@@ -70,12 +70,19 @@ Data Visualization
 
 Data can be visualized with any tools that display ROS topics, but we recommend `PlotJuggler <https://plotjuggler.io/>`_ for viewing high resolution live data, or data in bags.
 
-1. Open ``PlotJuggler`` running ``ros2 run plotjuggler plotjuggler``.
+1. Open ``PlotJuggler`` by running ``ros2 run plotjuggler plotjuggler`` from the command line.
+
    .. image:: images/plotjuggler.png
-2. Visualize the data:
-   - Importing from the ros2bag
-   - Subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+      :alt: PlotJuggler
+
+2. Visualize the data by importing from the ros2bag file or subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+
 3. Choose the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` from the popup window.
+
    .. image:: images/plotjuggler_select_topics.png
-4. Now, select the variables that are of your interest and drag them to the plot.
+      :alt: PlotJuggler Select Topics
+
+4. Then, select the variables that are of your interest and drag them to the plot.
+
    .. image:: images/plotjuggler_visualizing_data.png
+      :alt: PlotJuggler Visualizing Data

hardware_interface/include/hardware_interface/handle.hpp

@@ -319,7 +319,7 @@ class Handle
     // END
   }
 
-  std::shared_mutex & get_mutex() { return handle_mutex_; }
+  std::shared_mutex & get_mutex() const { return handle_mutex_; }
 
   HandleDataType get_data_type() const { return data_type_; }
 

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -527,6 +527,7 @@ class ResourceManager
   mutable std::recursive_mutex resource_interfaces_lock_;
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   mutable std::recursive_mutex resources_lock_;
+  mutable std::recursive_mutex joint_limiters_lock_;
 
 private:
   bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;

hardware_interface/src/resource_manager.cpp

@@ -700,6 +700,15 @@ class ResourceStorage
         if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
         {
           soft_limits = {hw_info.soft_limits.at(joint_name)};
+          RCLCPP_INFO(
+            get_logger(), "Using SoftJointLimiter for joint '%s' in hardware '%s' : '%s'",
+            joint_name.c_str(), hw_info.name.c_str(), soft_limits[0].to_string().c_str());
+        }
+        else
+        {
+          RCLCPP_INFO(
+            get_logger(), "Using JointLimiter for joint '%s' in hardware '%s' : '%s'",
+            joint_name.c_str(), hw_info.name.c_str(), limits.to_string().c_str());
         }
         std::unique_ptr<
           joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>
@@ -738,14 +747,19 @@ class ResourceStorage
       const std::string interface_name = joint_name + "/" + interface_type;
       if (interface_map.find(interface_name) != interface_map.end())
       {
-        // If the command interface is not claimed, then the value is not set
-        if (is_command_itf && !claimed_command_interface_map_.at(interface_name))
+        // If the command interface is not claimed, then the value is not set (or) if the
+        // interface doesn't exist, then value is not set
+        if (
+          is_command_itf && (claimed_command_interface_map_.count(interface_name) == 0 ||
+                             !claimed_command_interface_map_.at(interface_name)))
         {
           value = std::nullopt;
         }
         else
         {
-          value = interface_map.at(interface_name)->get_value();
+          auto itf_handle = interface_map.at(interface_name);
+          std::shared_lock<std::shared_mutex> lock(itf_handle->get_mutex());
+          value = itf_handle->get_optional(lock).value();
         }
       }
     };
@@ -767,7 +781,9 @@ class ResourceStorage
       const std::string interface_name = limited_command.joint_name + "/" + interface_type;
       if (data.has_value() && interface_map.find(interface_name) != interface_map.end())
       {
-        interface_map.at(interface_name)->set_value(data.value());
+        auto itf_handle = interface_map.at(interface_name);
+        std::unique_lock<std::shared_mutex> lock(itf_handle->get_mutex());
+        std::ignore = itf_handle->set_value(lock, data.value());
       }
     };
     // update the command data of the limiters
@@ -955,7 +971,8 @@ class ResourceStorage
               interface_name.c_str());
             continue;
           }
-          const auto limiter_fn = [&, interface_name](double value, bool & is_limited) -> double
+          const auto limiter_fn = [this, joint_name, interface_name, desired_period, &limiters](
+                                    double value, bool & is_limited) -> double
           {
             is_limited = false;
             joint_limits::JointInterfacesCommandLimiterData data;
@@ -987,7 +1004,9 @@ class ResourceStorage
             {
               RCLCPP_ERROR_THROTTLE(
                 get_logger(), *rm_clock_, 1000,
-                "Command of at least one joint is out of limits (throttled log).");
+                "Command of at least one joint is out of limits (throttled log). %s with desired "
+                "period : %f sec.",
+                data.to_string().c_str(), desired_period.seconds());
             }
             if (
               interface_name == hardware_interface::HW_IF_POSITION &&
@@ -1437,6 +1456,7 @@ bool ResourceManager::load_and_initialize_components(
 
 void ResourceManager::import_joint_limiters(const std::string & urdf)
 {
+  std::lock_guard<std::recursive_mutex> guard(joint_limiters_lock_);
   const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
   resource_storage_->import_joint_limiters(hardware_info);
 }
@@ -1958,6 +1978,21 @@ bool ResourceManager::perform_command_mode_switch(
   const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_);
   const bool systems_result = call_perform_mode_switch(resource_storage_->systems_);
 
+  if (actuators_result && systems_result)
+  {
+    // Reset the internals of the joint limiters
+    for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_)
+    {
+      for (const auto & [joint_name, limiter] : limiters)
+      {
+        limiter->reset_internals();
+        RCLCPP_DEBUG(
+          get_logger(), "Resetting internals of joint limiter for joint '%s' in hardware '%s'",
+          joint_name.c_str(), hw_name.c_str());
+      }
+    }
+  }
+
   return actuators_result && systems_result;
 }
 
@@ -2048,6 +2083,12 @@ return_type ResourceManager::set_component_state(
 // CM API: Called in "update"-thread
 bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period)
 {
+  std::unique_lock<std::recursive_mutex> limiters_guard(joint_limiters_lock_, std::try_to_lock);
+  if (!limiters_guard.owns_lock())
+  {
+    return false;
+  }
+
   bool enforce_result = false;
   // Joint Limiters operations
   for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_)

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -102,15 +102,15 @@ class TestActuator : public ActuatorInterface
     const std::vector<std::string> & /*start_interfaces*/,
     const std::vector<std::string> & /*stop_interfaces*/) override
   {
-    position_state_ += 1.0;
+    position_state_ += 0.001;
     return hardware_interface::return_type::OK;
   }
 
   hardware_interface::return_type perform_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,
     const std::vector<std::string> & /*stop_interfaces*/) override
   {
-    position_state_ += 100.0;
+    position_state_ += 0.1;
     return hardware_interface::return_type::OK;
   }
 

hardware_interface_testing/test/test_resource_manager.cpp

@@ -2562,8 +2562,8 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
-      claimed_itfs[0].set_value(10.0);
-      claimed_itfs[1].set_value(-20.0);
+      claimed_itfs[0].set_value(2.0);
+      claimed_itfs[1].set_value(-4.0);
 
       auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
       EXPECT_TRUE(ok_write);
@@ -2579,8 +2579,8 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
-      EXPECT_EQ(state_itfs[0].get_value(), 5.0);
-      EXPECT_EQ(state_itfs[1].get_value(), -10.0);
+      EXPECT_EQ(state_itfs[0].get_value(), 1.0);
+      EXPECT_EQ(state_itfs[1].get_value(), -2.0);
       auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
       EXPECT_TRUE(ok_write);
       EXPECT_TRUE(failed_hardware_names_write.empty());
@@ -2595,9 +2595,11 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
-      EXPECT_EQ(state_itfs[0].get_value(), 5.0);
-      EXPECT_EQ(state_itfs[1].get_value(), -10.0);
+      EXPECT_EQ(state_itfs[0].get_value(), 1.0);
+      EXPECT_EQ(state_itfs[1].get_value(), -2.0);
 
+      EXPECT_EQ(claimed_itfs[0].get_value(), 2.0);
+      EXPECT_EQ(claimed_itfs[1].get_value(), -4.0);
       claimed_itfs[0].set_value(0.0);
       claimed_itfs[1].set_value(0.0);
       EXPECT_EQ(claimed_itfs[0].get_value(), 0.0);
@@ -2606,9 +2608,9 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       // enforcing limits
       rm->enforce_command_limits(duration);
 
-      ASSERT_NEAR(state_itfs[2].get_value(), 5.05, 0.00001);
-      // it is limited to the M_PI as the actual position is outside the range
-      EXPECT_NEAR(claimed_itfs[0].get_value(), M_PI, 0.00001);
+      ASSERT_NEAR(state_itfs[2].get_value(), 1.05, 0.00001);
+      // It is using the actual velocity 1.05 to limit the claimed_itf
+      EXPECT_NEAR(claimed_itfs[0].get_value(), 1.048, 0.00001);
       EXPECT_EQ(claimed_itfs[1].get_value(), 0.0);
 
       auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
@@ -2621,33 +2623,30 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_TRUE(read_ok);
       EXPECT_TRUE(failed_hardware_names_read.empty());
 
-      ASSERT_NEAR(state_itfs[0].get_value(), M_PI_2, 0.00001);
+      ASSERT_NEAR(state_itfs[0].get_value(), claimed_itfs[0].get_value() / 2.0, 0.00001);
       ASSERT_EQ(state_itfs[1].get_value(), 0.0);
     }
 
     // Reset the position state interface of actuator to zero
     {
-      ASSERT_GT(state_itfs[2].get_value(), 5.05);
+      ASSERT_GT(state_itfs[2].get_value(), 1.05);
       claimed_itfs[0].set_value(test_constants::RESET_STATE_INTERFACES_VALUE);
       auto [read_ok, failed_hardware_names_read] = rm->read(time, duration);
       EXPECT_TRUE(read_ok);
       EXPECT_TRUE(failed_hardware_names_read.empty());
       ASSERT_EQ(state_itfs[2].get_value(), 0.0);
       claimed_itfs[0].set_value(0.0);
       claimed_itfs[1].set_value(0.0);
+      ASSERT_EQ(claimed_itfs[0].get_value(), 0.0);
+      ASSERT_EQ(claimed_itfs[1].get_value(), 0.0);
     }
 
     double new_state_value_1 = state_itfs[0].get_value();
     double new_state_value_2 = state_itfs[1].get_value();
     // Now loop and see that the joint limits are being enforced progressively
-    for (size_t i = 1; i < 2000; i++)
+    for (size_t i = 1; i < 300; i++)
     {
-      // let's amplify the limit enforce period, to test more rapidly. It would work with 0.01s as
-      // well
-      const rclcpp::Duration enforce_period =
-        rclcpp::Duration::from_seconds(duration.seconds() * 10.0);
-
-      auto [ok, failed_hardware_names] = rm->read(time, enforce_period);
+      auto [ok, failed_hardware_names] = rm->read(time, duration);
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
@@ -2660,13 +2659,15 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_EQ(claimed_itfs[1].get_value(), -20.0);
 
       // enforcing limits
-      rm->enforce_command_limits(enforce_period);
+      rm->enforce_command_limits(duration);
 
       // the joint limits value is same as in the parsed URDF
       const double velocity_joint_1 = 0.2;
+      const double prev_command_val = 1.048;
       ASSERT_NEAR(
         claimed_itfs[0].get_value(),
-        std::min(state_itfs[2].get_value() + (velocity_joint_1 * enforce_period.seconds()), M_PI),
+        prev_command_val +
+          std::min((velocity_joint_1 * (duration.seconds() * static_cast<double>(i))), M_PI),
         1.0e-8)
         << "This should be progressively increasing as it is a position limit for iteration : "
         << i;
@@ -2677,7 +2678,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       new_state_value_1 = claimed_itfs[0].get_value() / 2.0;
       new_state_value_2 = claimed_itfs[1].get_value() / 2.0;
 
-      auto [ok_write, failed_hardware_names_write] = rm->write(time, enforce_period);
+      auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
       EXPECT_TRUE(ok_write);
       EXPECT_TRUE(failed_hardware_names_write.empty());
       node_.get_clock()->sleep_until(time + duration);
@@ -2688,7 +2689,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
-      EXPECT_NEAR(state_itfs[0].get_value(), M_PI_2, 0.00001);
+      EXPECT_NEAR(state_itfs[0].get_value(), 0.823, 0.00001);
       EXPECT_NEAR(state_itfs[1].get_value(), -0.1, 0.00001);
     }
   }