Skip to content

Commit

Permalink
Component parser: Get mimic information from URDF (#1256)
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich authored Mar 28, 2024
1 parent 595567a commit 5ae5cc3
Show file tree
Hide file tree
Showing 27 changed files with 1,080 additions and 372 deletions.
17 changes: 9 additions & 8 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
-----------

Expand Down Expand Up @@ -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
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

Expand Down
51 changes: 51 additions & 0 deletions doc/migration/Foxy.rst
Original file line number Diff line number Diff line change
@@ -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``
50 changes: 50 additions & 0 deletions doc/migration/Iron.rst
Original file line number Diff line number Diff line change
@@ -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>
1 change: 1 addition & 0 deletions hardware_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rcutils
TinyXML2
tinyxml2_vendor
urdf
)

find_package(ament_cmake REQUIRED)
Expand Down
50 changes: 0 additions & 50 deletions hardware_interface/doc/hardware_components_userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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``
2 changes: 2 additions & 0 deletions hardware_interface/doc/hardware_interface_types_userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
19 changes: 6 additions & 13 deletions hardware_interface/doc/mock_components_userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
,,,,,,,,,,,,,,,,,,,,,,,,

Expand Down
32 changes: 28 additions & 4 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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;
Expand Down
8 changes: 0 additions & 8 deletions hardware_interface/include/mock_components/generic_system.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Loading

0 comments on commit 5ae5cc3

Please sign in to comment.