Merge branch 'master' into propagate/logger

ros-controls · Jul 4, 2024 · 2bf8a1d · 2bf8a1d
2 parents 5bc3c93 + bb85eae
commit 2bf8a1d
Show file tree

Hide file tree

Showing 40 changed files with 2,109 additions and 181 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -37,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.15.2
+    rev: v3.16.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -56,14 +56,14 @@ repos:
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.0.0
+    rev: 7.1.0
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.5
+    rev: v18.1.8
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.28.4
+    rev: 0.28.6
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+
 4.11.0 (2024-05-14)
 -------------------
 * Fix dependencies for source build (`#1533 <https://github.com/ros-controls/ros2_control/issues/1533>`_)

diff --git a/controller_interface/package.xml b/controller_interface/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+* [rqt_controller_manager] Add hardware components (`#1455 <https://github.com/ros-controls/ros2_control/issues/1455>`_)
+* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 <https://github.com/ros-controls/ros2_control/issues/1354>`_)
+* Fix update `period` for the first update after activation (`#1551 <https://github.com/ros-controls/ros2_control/issues/1551>`_)
+* Bump version of pre-commit hooks (`#1556 <https://github.com/ros-controls/ros2_control/issues/1556>`_)
+* Contributors: Christoph Fröhlich, Dr. Denis, github-actions[bot]
+
 4.11.0 (2024-05-14)
 -------------------
 * Add find_package for ament_cmake_gen_version_h (`#1534 <https://github.com/ros-controls/ros2_control/issues/1534>`_)

diff --git a/controller_manager/package.xml b/controller_manager/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_manager</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+
 4.11.0 (2024-05-14)
 -------------------
 

diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>controller_manager_msgs</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst
@@ -2,6 +2,17 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+* Add resources_lock\_ lock_guards to avoid race condition when loading robot_description through topic (`#1451 <https://github.com/ros-controls/ros2_control/issues/1451>`_)
+* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 <https://github.com/ros-controls/ros2_control/issues/1354>`_)
+* Small improvements to the error output in component parser to make debugging easier. (`#1580 <https://github.com/ros-controls/ros2_control/issues/1580>`_)
+* Fix link to gazebosim.org (`#1563 <https://github.com/ros-controls/ros2_control/issues/1563>`_)
+* Add doc page about joint kinematics (`#1497 <https://github.com/ros-controls/ros2_control/issues/1497>`_)
+* Bump version of pre-commit hooks (`#1556 <https://github.com/ros-controls/ros2_control/issues/1556>`_)
+* [Feature] Hardware Components Grouping (`#1458 <https://github.com/ros-controls/ros2_control/issues/1458>`_)
+* Contributors: Christoph Fröhlich, Dr. Denis, Sai Kishor Kothakota, github-actions[bot]
+
 4.11.0 (2024-05-14)
 -------------------
 * Add find_package for ament_cmake_gen_version_h (`#1534 <https://github.com/ros-controls/ros2_control/issues/1534>`_)

diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>ros2_control hardware interface</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
@@ -838,98 +838,53 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
     for (auto i = 0u; i < hw_info.joints.size(); ++i)
     {
       const auto & joint = hw_info.joints.at(i);
-
-      // Search for mimic joints defined in ros2_control tag (deprecated)
-      // TODO(christophfroehlich) delete deprecated config with ROS-J
-      if (joint.parameters.find("mimic") != joint.parameters.cend())
+      auto urdf_joint = model.getJoint(joint.name);
+      if (!urdf_joint)
       {
-        std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. "
-                  << "Please define mimic joints in URDF." << std::endl;
-        const auto mimicked_joint_it = std::find_if(
-          hw_info.joints.begin(), hw_info.joints.end(),
-          [&mimicked_joint =
-             joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info)
-          { return joint_info.name == mimicked_joint; });
-        if (mimicked_joint_it == hw_info.joints.cend())
-        {
-          throw std::runtime_error(
-            "Mimicked joint '" + joint.parameters.at("mimic") + "' not found");
-        }
-        hardware_interface::MimicJoint mimic_joint;
-        mimic_joint.joint_index = i;
-        mimic_joint.multiplier = 1.0;
-        mimic_joint.offset = 0.0;
-        mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it);
-        auto param_it = joint.parameters.find("multiplier");
-        if (param_it != joint.parameters.end())
-        {
-          mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier"));
-        }
-        param_it = joint.parameters.find("offset");
-        if (param_it != joint.parameters.end())
-        {
-          mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset"));
-        }
-        hw_info.mimic_joints.push_back(mimic_joint);
+        throw std::runtime_error("Joint '" + joint.name + "' not found in URDF");
       }
-      else
+      if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE)
       {
-        auto urdf_joint = model.getJoint(joint.name);
-        if (!urdf_joint)
-        {
-          throw std::runtime_error("Joint '" + joint.name + "' not found in URDF");
-        }
-        if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE)
+        throw std::runtime_error(
+          "Joint '" + joint.name + "' has no mimic information in the URDF.");
+      }
+      if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE)
+      {
+        if (joint.command_interfaces.size() > 0)
         {
           throw std::runtime_error(
-            "Joint '" + joint.name + "' has no mimic information in the URDF.");
+            "Joint '" + joint.name +
+            "' has mimic attribute not set to false: Activated mimic joints cannot have command "
+            "interfaces.");
         }
-        if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE)
+        auto find_joint = [&hw_info](const std::string & name)
         {
-          if (joint.command_interfaces.size() > 0)
+          auto it = std::find_if(
+            hw_info.joints.begin(), hw_info.joints.end(),
+            [&name](const auto & j) { return j.name == name; });
+          if (it == hw_info.joints.end())
           {
-            throw std::runtime_error(
-              "Joint '" + joint.name +
-              "' has mimic attribute not set to false: Activated mimic joints cannot have command "
-              "interfaces.");
+            throw std::runtime_error("Mimic joint '" + name + "' not found in <ros2_control> tag");
           }
-          auto find_joint = [&hw_info](const std::string & name)
-          {
-            auto it = std::find_if(
-              hw_info.joints.begin(), hw_info.joints.end(),
-              [&name](const auto & j) { return j.name == name; });
-            if (it == hw_info.joints.end())
-            {
-              throw std::runtime_error(
-                "Mimic joint '" + name + "' not found in <ros2_control> tag");
-            }
-            return std::distance(hw_info.joints.begin(), it);
-          };
-
-          MimicJoint mimic_joint;
-          mimic_joint.joint_index = i;
-          mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
-          mimic_joint.multiplier = urdf_joint->mimic->multiplier;
-          mimic_joint.offset = urdf_joint->mimic->offset;
-          hw_info.mimic_joints.push_back(mimic_joint);
-        }
-      }
-      // TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch
-      // from above is removed (double check here, but throws already above if not found in URDF)
-      auto urdf_joint = model.getJoint(joint.name);
-      if (!urdf_joint)
-      {
-        std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!"
-                  << std::endl;
-        continue;
+          return std::distance(hw_info.joints.begin(), it);
+        };
+
+        MimicJoint mimic_joint;
+        mimic_joint.joint_index = i;
+        mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
+        mimic_joint.multiplier = urdf_joint->mimic->multiplier;
+        mimic_joint.offset = urdf_joint->mimic->offset;
+        hw_info.mimic_joints.push_back(mimic_joint);
       }
+
       if (urdf_joint->type == urdf::Joint::FIXED)
       {
         throw std::runtime_error(
           "Joint '" + joint.name +
           "' is of type 'fixed'. "
           "Fixed joints do not make sense in ros2_control.");
       }
+
       joint_limits::JointLimits limits;
       getJointLimits(urdf_joint, limits);
       // Take the most restricted one. Also valid for continuous-joint type only

diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1031,6 +1031,7 @@ bool ResourceManager::load_and_initialize_components(
   const std::string sensor_type = "sensor";
   const std::string actuator_type = "actuator";
 
+  std::lock_guard<std::recursive_mutex> resource_guard(resources_lock_);
   for (const auto & individual_hardware_info : hardware_info)
   {
     // Check for identical names
@@ -1614,6 +1615,7 @@ return_type ResourceManager::set_component_state(
     return false;
   };
 
+  std::lock_guard<std::recursive_mutex> guard(resources_lock_);
   bool found = find_set_component_state(
     std::bind(&ResourceStorage::set_component_state<Actuator>, resource_storage_.get(), _1, _2),
     resource_storage_->actuators_);

diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
@@ -1341,35 +1341,6 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
   EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
 }
 
-// TODO(christophfroehlich) delete deprecated config test
-TEST_F(TestComponentParser, gripper_mimic_deprecated_valid_config)
-{
-  const auto urdf_to_test =
-    std::string(ros2_control_test_assets::gripper_urdf_head) +
-    std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated) +
-    std::string(ros2_control_test_assets::urdf_tail);
-  std::vector<hardware_interface::HardwareInfo> hw_info;
-  ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test));
-  ASSERT_THAT(hw_info, SizeIs(1));
-  ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1));
-  EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0);
-  EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
-  EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
-  EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
-}
-
-TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error)
-{
-  const auto urdf_to_test =
-    std::string(ros2_control_test_assets::gripper_urdf_head) +
-    std::string(
-      ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated_unknown_joint) +
-    std::string(ros2_control_test_assets::urdf_tail);
-  std::vector<hardware_interface::HardwareInfo> hw_info;
-  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
-}
-// end delete deprecated config test
-
 TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error)
 {
   const auto urdf_to_test =

diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package hardware_interface_testing
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 <https://github.com/ros-controls/ros2_control/issues/1354>`_)
+* Contributors: Dr. Denis
+
 4.11.0 (2024-05-14)
 -------------------
 

diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface_testing</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>ros2_control hardware interface testing</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package joint_limits
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+* Reactivate generate_version_header (`#1544 <https://github.com/ros-controls/ros2_control/issues/1544>`_)
+* Bump version of pre-commit hooks (`#1556 <https://github.com/ros-controls/ros2_control/issues/1556>`_)
+* Contributors: Christoph Fröhlich, github-actions[bot]
+
 4.11.0 (2024-05-14)
 -------------------
 * Fix dependencies for source build (`#1533 <https://github.com/ros-controls/ros2_control/issues/1533>`_)