Port four bar linkage transmission loader from ROS1

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -84,6 +84,7 @@ struct ActuatorInfo
   std::string name;
   std::vector<std::string> interfaces;
   std::string role;
+  double mechanical_reduction = 1.0;
   double offset = 0.0;
 };
 

hardware_interface/src/component_parser.cpp

@@ -385,6 +385,8 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e
   ActuatorInfo actuator_info;
   actuator_info.name = get_attribute_value(element_it, kNameAttribute, element_it->Name());
   actuator_info.role = get_attribute_value(element_it, kRoleAttribute, element_it->Name());
+  actuator_info.mechanical_reduction =
+    get_parameter_value_or(element_it->FirstChildElement(), kReductionAttribute, 1.0);
   actuator_info.offset =
     get_parameter_value_or(element_it->FirstChildElement(), kOffsetAttribute, 0.0);
   return actuator_info;
@@ -474,7 +476,7 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware)
       }
 
       transmission.actuators.push_back(
-        ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 0.0});
+        ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 1.0, 0.0});
     }
   }
 }

transmission_interface/CMakeLists.txt

@@ -19,7 +19,10 @@ install(
 add_library(transmission_interface SHARED
   include/transmission_interface/transmission_loader.hpp
   include/transmission_interface/simple_transmission_loader.hpp
+  include/transmission_interface/four_bar_linkage_transmission_loader.hpp
+
   src/simple_transmission_loader.cpp
+  src/four_bar_linkage_transmission_loader.cpp
 )
 
 target_include_directories(transmission_interface PUBLIC include)
@@ -76,6 +79,13 @@ if(BUILD_TESTING)
   )
   target_include_directories(test_simple_transmission_loader PUBLIC include hardware_interface)
   ament_target_dependencies(test_simple_transmission_loader hardware_interface)
+
+  ament_add_gmock(
+    test_four_bar_linkage_transmission_loader
+    test/four_bar_linkage_transmission_loader_test.cpp
+  )
+  target_include_directories(test_four_bar_linkage_transmission_loader PUBLIC include hardware_interface)
+  ament_target_dependencies(test_four_bar_linkage_transmission_loader hardware_interface)
 endif()
 
 ament_export_include_directories(

transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp

@@ -0,0 +1,50 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2022, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of PAL Robotics S.L. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
+#define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
+
+#include <memory>
+
+#include "transmission_interface/transmission.hpp"
+#include "transmission_interface/transmission_loader.hpp"
+
+namespace transmission_interface
+{
+/**
+ * \brief Class for loading a four-bar linkage transmission instance from configuration data.
+ */
+class FourBarLinkageTransmissionLoader : public TransmissionLoader
+{
+public:
+  std::shared_ptr<Transmission> load(
+    const hardware_interface::TransmissionInfo & transmission_info) override;
+};
+
+}  // namespace transmission_interface
+
+#endif  // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_

transmission_interface/ros2_control_plugins.xml

@@ -7,4 +7,12 @@
       Load configuration of a simple transmission from a URDF description.
     </description>
   </class>
+
+  <class name="transmission_interface/FourBarLinkageTransmission"
+         type="transmission_interface::FourBarLinkageTransmissionLoader"
+         base_class_type="transmission_interface::TransmissionLoader">
+    <description>
+      Load configuration of a four bar linkage transmission from a URDF description.
+    </description>
+  </class>
 </library>

transmission_interface/src/four_bar_linkage_transmission_loader.cpp

@@ -0,0 +1,61 @@
+// Copyright 2022 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "transmission_interface/four_bar_linkage_transmission_loader.hpp"
+
+#include <memory>
+
+#include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "transmission_interface/four_bar_linkage_transmission.hpp"
+
+namespace transmission_interface
+{
+std::shared_ptr<Transmission> FourBarLinkageTransmissionLoader::load(
+  const hardware_interface::TransmissionInfo & transmission_info)
+{
+  try
+  {
+    const auto act_reduction1 = transmission_info.actuators.at(0).mechanical_reduction;
+    const auto act_reduction2 = transmission_info.actuators.at(1).mechanical_reduction;
+
+    const auto jnt_reduction1 = transmission_info.joints.at(0).mechanical_reduction;
+    const auto jnt_reduction2 = transmission_info.joints.at(1).mechanical_reduction;
+
+    const auto jnt_offset1 = transmission_info.joints.at(0).offset;
+    const auto jnt_offset2 = transmission_info.joints.at(1).offset;
+
+    std::shared_ptr<Transmission> transmission(new FourBarLinkageTransmission(
+      {act_reduction1, act_reduction2}, {jnt_reduction1, jnt_reduction2},
+      {jnt_offset1, jnt_offset2}));
+    return transmission;
+  }
+  catch (const Exception & ex)
+  {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("four_bar_linkage_transmission_loader"),
+      "Failed to construct transmission '%s'", ex.what());
+    return std::shared_ptr<Transmission>();
+  }
+}
+
+}  // namespace transmission_interface
+
+PLUGINLIB_EXPORT_CLASS(
+  transmission_interface::FourBarLinkageTransmissionLoader,
+  transmission_interface::TransmissionLoader)