Merge branch 'master' into isolate/start_stop_interfaces

Author: christophfroehlich

controller_manager/src/controller_manager.cpp

@@ -499,7 +499,10 @@ void ControllerManager::init_controller_manager()
   // Get parameters needed for RT "update" loop to work
   if (is_resource_manager_initialized())
   {
-    resource_manager_->import_joint_limiters(robot_description_);
+    if (enforce_command_limits_)
+    {
+      resource_manager_->import_joint_limiters(robot_description_);
+    }
     init_services();
   }
   else
@@ -621,7 +624,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  resource_manager_->import_joint_limiters(robot_description_);
+  if (enforce_command_limits_)
+  {
+    resource_manager_->import_joint_limiters(robot_description_);
+  }
   if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
   {
     RCLCPP_WARN(
@@ -2880,11 +2886,7 @@ controller_interface::return_type ControllerManager::update(
     // To publish the activity of the failing controllers and the fallback controllers
     publish_activity();
   }
-
-  if (enforce_command_limits_)
-  {
-    resource_manager_->enforce_command_limits(period);
-  }
+  resource_manager_->enforce_command_limits(period);
 
   // there are controllers to (de)activate
   if (switch_params_.do_switch)

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/src/resource_manager.cpp

@@ -756,7 +756,9 @@ class ResourceStorage
         }
         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();
         }
       }
     };
@@ -778,7 +780,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
@@ -966,7 +970,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;
@@ -994,10 +999,14 @@ class ResourceStorage
             }
             data.limited = data.command;
             is_limited = limiters[joint_name]->enforce(data.actual, data.limited, desired_period);
-            RCLCPP_ERROR_THROTTLE(
-              get_logger(), *rm_clock_, 1000,
-              "Command '%s' for joint '%s' is out of limits. Command limited to %f - %d",
-              interface_name.c_str(), joint_name.c_str(), value, is_limited);
+            if (is_limited)
+            {
+              RCLCPP_ERROR_THROTTLE(
+                get_logger(), *rm_clock_, 1000,
+                "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 &&
               data.limited.position.has_value())

joint_limits/include/joint_limits/data_structures.hpp

@@ -64,6 +64,41 @@ struct JointControlInterfacesData
   bool has_acceleration() const { return acceleration.has_value(); }
 
   bool has_jerk() const { return jerk.has_value(); }
+
+  std::string to_string() const
+  {
+    std::string str;
+    if (has_position())
+    {
+      str += "position: " + std::to_string(position.value()) + ", ";
+    }
+    if (has_velocity())
+    {
+      str += "velocity: " + std::to_string(velocity.value()) + ", ";
+    }
+    if (has_effort())
+    {
+      str += "effort: " + std::to_string(effort.value()) + ", ";
+    }
+    if (has_acceleration())
+    {
+      str += "acceleration: " + std::to_string(acceleration.value()) + ", ";
+    }
+    if (has_jerk())
+    {
+      str += "jerk: " + std::to_string(jerk.value());
+    }
+    // trim the last comma and space
+    if (!str.empty() && str.back() == ' ')
+    {
+      str.pop_back();
+    }
+    if (!str.empty() && str.back() == ',')
+    {
+      str.pop_back();
+    }
+    return str;
+  }
 };
 
 struct JointInterfacesCommandLimiterData
@@ -77,6 +112,14 @@ struct JointInterfacesCommandLimiterData
   bool has_actual_data() const { return actual.has_data(); }
 
   bool has_command_data() const { return command.has_data(); }
+
+  bool has_limited_data() const { return limited.has_data(); }
+
+  std::string to_string() const
+  {
+    return "Joint : '" + joint_name + "', (actual: [" + actual.to_string() + "], command : [" +
+           command.to_string() + "], limited: [" + limited.to_string() + "])";
+  }
 };
 
 }  // namespace joint_limits