Component parser: Get mimic information from URDF (#1256)

controller_manager/doc/userdoc.rst

@@ -45,6 +45,15 @@ Alternatives to the standard kernel include
 Though installing a realtime-kernel will definitely get the best results when it comes to low
 jitter, using a lowlatency kernel can improve things a lot with being really easy to install.
 
+Subscribers
+-----------
+
+~/robot_description [std_msgs::msg::String]
+  String with the URDF xml, e.g., from ``robot_state_publisher``.
+  Reloading of the URDF is not supported yet.
+  All joints defined in the ``<ros2_control>``-tag have to be present in the URDF.
+
+
 Parameters
 -----------
 
@@ -74,18 +83,10 @@ update_rate (mandatory; integer)
   The frequency of controller manager's real-time update loop.
   This loop reads states from hardware, updates controller and writes commands to hardware.
 
-
 <controller_name>.type
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.
 
-Subscribers
------------
-
-robot_description (std_msgs/msg/String)
-  The URDF string as robot description.
-  This is usually published by the ``robot_state_publisher`` node.
-
 Handling Multiple Controller Managers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

doc/migration/Foxy.rst

@@ -0,0 +1,51 @@
+Foxy to Galactic
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+hardware_interface
+**************************************
+Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle.
+The following list shows a set of quick changes to port existing hardware components to Galactic:
+
+1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn``
+
+2. If using BaseInterface as base class then you should remove it. Specifically, change:
+
+  .. code-block:: c++
+
+    hardware_interface::BaseInterface<hardware_interface::[Actuator|Sensor|System]Interface>
+
+  to
+
+  .. code-block:: c++
+
+    hardware_interface::[Actuator|Sensor|System]Interface
+
+3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp``
+
+4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary
+
+5. replace first three lines in ``on_init`` to
+
+  .. code-block:: c++
+
+    if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS)
+    {
+      return CallbackReturn::ERROR;
+    }
+
+
+6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;``
+
+7. Remove all lines with ``status_ = ...`` or ``status::...``
+
+8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and
+   ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)``
+
+9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn``
+
+10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;``
+
+11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR``
+
+12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add
+    ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml``

doc/migration/Iron.rst

@@ -0,0 +1,50 @@
+Iron to Jazzy
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+component parser
+*****************
+Changes from `(PR #1256) <https://github.com/ros-controls/ros2_control/pull/1256>`__
+
+* All ``joints`` defined in the ``<ros2_control>``-tag have to be present in the URDF received :ref:`by the controller manager <doc/ros2_control/controller_manager/doc/userdoc:subscribers>`, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ``<ros2_control>``-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead.
+* The syntax for mimic joints is changed to the `official URDF specification <https://wiki.ros.org/urdf/XML/joint>`__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of
+
+  .. code-block:: xml
+
+    <ros2_control name="GazeboSystem" type="system">
+      <joint name="right_finger_joint">
+        <command_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.15</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_finger_joint">
+        <param name="mimic">right_finger_joint</param>
+        <param name="multiplier">1</param>
+        <command_interface name="position"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+    </ros2_control>
+
+  define your mimic joints directly in the joint definitions:
+
+  .. code-block:: xml
+
+    <joint name="right_finger_joint" type="prismatic">
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
+      <parent link="base"/>
+      <child link="finger_right"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>
+    <joint name="left_finger_joint" type="prismatic">
+      <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
+      <parent link="base"/>
+      <child link="finger_left"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>

hardware_interface/CMakeLists.txt

@@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   TinyXML2
   tinyxml2_vendor
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)

hardware_interface/doc/hardware_components_userdoc.rst

@@ -27,53 +27,3 @@ If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``w
 Error handling follows the `node lifecycle <https://design.ros2.org/articles/node_lifecycle.html>`_.
 If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED``  state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered.
 The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager.
-
-Migration from Foxy to newer versions
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle.
-The following list shows a set of quick changes to port existing hardware components to Galactic:
-
-1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn``
-
-2. If using BaseInterface as base class then you should remove it. Specifically, change:
-
-  .. code-block:: c++
-
-    hardware_interface::BaseInterface<hardware_interface::[Actuator|Sensor|System]Interface>
-
-  to
-
-  .. code-block:: c++
-
-    hardware_interface::[Actuator|Sensor|System]Interface
-
-3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp``
-
-4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary
-
-5. replace first three lines in ``on_init`` to
-
-  .. code-block:: c++
-
-    if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS)
-    {
-      return CallbackReturn::ERROR;
-    }
-
-
-6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;``
-
-7. Remove all lines with ``status_ = ...`` or ``status::...``
-
-8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and
-   ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)``
-
-9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn``
-
-10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;``
-
-11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR``
-
-12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add
-    ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml``

hardware_interface/doc/hardware_interface_types_userdoc.rst

@@ -14,6 +14,8 @@ Joints
 ``<joint>``-tag groups the interfaces associated with the joints of physical robots and actuators.
 They have command and state interfaces to set the goal values for hardware and read its current state.
 
+All joints defined in the ``<ros2_control>``-tag have to be present in the URDF received :ref:`by the controller manager <doc/ros2_control/controller_manager/doc/userdoc:subscribers>`.
+
 State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster <joint_state_broadcaster_userdoc>`
 
 Sensors

hardware_interface/doc/mock_components_userdoc.rst

@@ -19,9 +19,10 @@ For more information about hardware components check :ref:`detailed documentatio
 
 Features:
 
-  - support for mimic joints
+  - support for mimic joints, which is parsed from the URDF (see the `URDF wiki <http://wiki.ros.org/urdf/XML/joint>`__)
   - mirroring commands to states with and without offset
   - fake command interfaces for setting sensor data from an external node (combined with a :ref:`forward controller <forward_command_controller_userdoc>`)
+  - fake gpio interfaces for setting sensor data from an external node (combined with a :ref:`forward controller <forward_command_controller_userdoc>`)
 
 
 Parameters
@@ -36,24 +37,16 @@ mock_sensor_commands (optional; boolean; default: false)
   Creates fake command interfaces for faking sensor measurements with an external command.
   Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.
 
+mock_gpio_commands (optional; boolean; default: false)
+  Creates fake command interfaces for faking GPIO states with an external command.
+  Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.
+
 position_state_following_offset (optional; double; default: 0.0)
   Following offset added to the commanded values when mirrored to states.
 
-
 custom_interface_with_following_offset (optional; string; default: "")
   Mapping of offsetted commands to a custom interface.
 
-
-Per-joint Parameters
-,,,,,,,,,,,,,,,,,,,,
-
-mimic (optional; string)
-  Defined name of the joint to mimic. This is often used concept with parallel grippers. Example: ``<param name="mimic">joint1</param>``.
-
-
-multiplier (optional; double; default: 1; used if mimic joint is defined)
-  Multiplier of values for mimicking joint defined in ``mimic`` parameter. Example: ``<param name="multiplier">-2</param>``.
-
 Per-interface Parameters
 ,,,,,,,,,,,,,,,,,,,,,,,,
 

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -44,6 +44,23 @@ struct InterfaceInfo
   int size;
 };
 
+/// @brief This structure stores information about a joint that is mimicking another joint
+struct MimicJoint
+{
+  std::size_t joint_index;
+  std::size_t mimicked_joint_index;
+  double multiplier = 1.0;
+  double offset = 0.0;
+};
+
+/// @brief This enum is used to store the mimic attribute of a joint
+enum class MimicAttribute
+{
+  NOT_SET,
+  TRUE,
+  FALSE
+};
+
 /**
  * This structure stores information about components defined for a specific hardware
  * in robot's URDF.
@@ -55,6 +72,9 @@ struct ComponentInfo
   /// Type of the component: sensor, joint, or GPIO.
   std::string type;
 
+  ///  Hold the value of the mimic attribute if given, NOT_SET otherwise
+  MimicAttribute is_mimic = MimicAttribute::NOT_SET;
+
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
    * Used by joints and GPIOs.
@@ -116,22 +136,26 @@ struct HardwareInfo
   /// (Optional) Key-value pairs for hardware parameters.
   std::unordered_map<std::string, std::string> hardware_parameters;
   /**
-   * Map of joints provided by the hardware where the key is the joint name.
+   * Vector of joints provided by the hardware.
    * Required for Actuator and System Hardware.
    */
   std::vector<ComponentInfo> joints;
   /**
-   * Map of sensors provided by the hardware where the key is the joint or link name.
+   * Vector of mimic joints.
+   */
+  std::vector<MimicJoint> mimic_joints;
+  /**
+   * Vector of sensors provided by the hardware.
    * Required for Sensor and optional for System Hardware.
    */
   std::vector<ComponentInfo> sensors;
   /**
-   * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO.
+   * Vector of GPIOs provided by the hardware.
    * Optional for any hardware components.
    */
   std::vector<ComponentInfo> gpios;
   /**
-   * Map of transmissions to calculate ration between joints and physical actuators.
+   * Vector of transmissions to calculate ratio between joints and physical actuators.
    * Optional for Actuator and System Hardware.
    */
   std::vector<TransmissionInfo> transmissions;

hardware_interface/include/mock_components/generic_system.hpp

@@ -72,14 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
     hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
     hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
 
-  struct MimicJoint
-  {
-    std::size_t joint_index;
-    std::size_t mimicked_joint_index;
-    double multiplier = 1.0;
-  };
-  std::vector<MimicJoint> mimic_joints_;
-
   /// The size of this vector is (standard_interfaces_.size() x nr_joints)
   std::vector<std::vector<double>> joint_commands_;
   std::vector<std::vector<double>> joint_states_;

hardware_interface/package.xml

@@ -16,6 +16,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>tinyxml2_vendor</depend>
+  <depend>urdf</depend>
 
   <build_depend>rcutils</build_depend>
   <exec_depend>rcutils</exec_depend>