Skip to content

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
rhaschke committed Mar 11, 2024
1 parent 0e06553 commit b99848d
Showing 1 changed file with 30 additions and 35 deletions.
65 changes: 30 additions & 35 deletions doc/controller_configuration/controller_configuration_tutorial.rst
Original file line number Diff line number Diff line change
Expand Up @@ -130,52 +130,35 @@ Additional options for tuning the behavior and safety checks of MoveIt's executi
* ``allowed_goal_duration_margin``: same as above, but configured globally as a default for all controllers.
* ``allowed_start_tolerance``: joint state tolerance when validating that a trajectory's first point matches current robot state. If set to ``0`` MoveIt will skip waiting for the robot to stop after execution.

.. _ros control manager:

ROS Control Controller Manager
-------------------------------------
------------------------------

Alternatively to the simple controller manager described above, MoveIt also provides a controller manager that directly interfaces the `ROS Controller Manager <http://wiki.ros.org/controller_manager>`_. Instead of using a bridging configuration file like ``simple_moveit_controllers.yaml``, this controller manager directly queries the ROS Controller Manager for available controllers.

The *MoveIt ROS Control Controller Manager* keeps track of all loaded and started ROS controllers, as well as the subset these controllers that can be used with MoveIt. All loaded and started controllers are designated as *active*, and the subset of these controllers that can be used with MoveIt because they have associated controller handles is designated as *managed*.
The *MoveIt ROS Control Controller Manager* keeps track of all loaded and started ROS controllers, as well as the subset of these controllers that can be used with MoveIt. All loaded and started controllers are designated as *active*, and the subset of these controllers that can be used with MoveIt because they have associated controller handles is designated as *managed*.

Multi Controller Manager
-------------------------------
------------------------

The *MoveIt ROS Control Controller Manager* can only interface controllers from the single ROS controller manager found in the ROS namespace defined by the ROS parameter ``~ros_control_namespace`` (defaults to ``/``).

To overcome this limitation, there also exists ``MoveItMultiControllerManager``, which queries *all* existing ROS controller managers and instantiates all controllers with their respective namespace taking care of proper delegation.

This type of manager can be configured by setting ``moveit_controller_manager`` to ``moveit_ros_control_interface::MoveItMultiControllerManager``:

.. code-block:: XML
<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItMultiControllerManager" />
.. note::
The `Simple Controller Manager` is generic as it can interface controllers from multiple ROS controller managers by specifying different prefix names in the bridging configuration file.

Controller Switching and Namespaces
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

All controller names get prefixed by the namespace of their `ROS Control <http://wiki.ros.org/ros_control>`_ node. For this reason, controller names should not contain slashes.

For a particular `ROS Control <http://wiki.ros.org/ros_control>`_ node, MoveIt can decide which controllers to start or stop. MoveIt will take care of stopping controllers based on their *claimed joint resources* if a to-be-started controller needs any of those resources.

Remapping ``/joint_states`` topic
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
MoveIt requires joint states to be published on the ``/joint_states`` topic to internally maintain the robot's state.
If the joint states are published on another topic specific to your project, such as ``/robot/joint_states``, add a ``remap`` to the ``move_group`` node in ``move_group.launch`` file generated by MSA:

.. code-block:: XML
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<remap
from="joint_states"
to="robot/joint_states"
/>
<!-- Other settings -->
</node>
Fake Controller Manager
-----------------------

Expand Down Expand Up @@ -220,13 +203,13 @@ MoveIt Controller Managers only support controllers that implement the ``FollowJ

This is because only a `ControllerHandleAllocator <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp>`_ for this action type is `exported <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml>`_ as a plugin. Even though there is a `ControllerHandle <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h>`__ for ``GripperCommand`` actions, a corresponding ``ControllerHandleAllocator`` plugin that enables the controller handle to be dynamically created from the ROS controller type name, does not exist.

ROS Controllers with Joint Trajectory Action interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
ROS Controllers with a ``FollowJointTrajectory`` action interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

*Controller handles* implemented by MoveIt bridge ROS Controllers with the MoveIt motion planning pipeline by means of an `Action Client <http://wiki.ros.org/actionlib>`_, as long as the controller starts an *Action Server* that handles one of the two types of supported action interfaces:

* The `Joint Trajectory Controller Handle <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>`_ can be used for controllers that support `Follow Joint Trajectory Action <https://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html>`_.
* The `Gripper Controller Handle <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h>`_ can be used for controllers that support `Gripper Command Action <https://docs.ros.org/en/jade/api/control_msgs/html/action/GripperCommand.html>`_.
* The `Joint Trajectory Controller Handle <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>`_ can be used for controllers that support the `Follow Joint Trajectory Action <https://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html>`_.
* The `Gripper Controller Handle <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h>`_ can be used for controllers that support the `Gripper Command Action <https://docs.ros.org/en/jade/api/control_msgs/html/action/GripperCommand.html>`_.

The *MoveIt ROS Control Controller Manager* will regard any controllers loaded by ROS Controller Manager as *managed* if it finds a plugin registration that links the ``type`` of the ROS controller with a MoveIt Controller Handle Allocator. If no such registration is found, the controller is regarded as *unmanaged* (merely *active*) and cannot be used to receive trajectory commands from MoveIt.

Expand All @@ -240,7 +223,7 @@ First, create a plugin description file:
<library path="libmoveit_ros_control_interface_trajectory_plugin">
<class
name="controller_package_name/controller_type_name"
name="my_controller_package_name/my_controller_type_name"
type="moveit_ros_control_interface::JointTrajectoryControllerAllocator"
base_class_type="moveit_ros_control_interface::ControllerHandleAllocator"
>
Expand All @@ -250,7 +233,7 @@ First, create a plugin description file:
</class>
</library>
Reference the plugin description in your package.xml's ``export`` section:
Reference this plugin description file in your package.xml's ``export`` section:

.. code-block:: XML
Expand All @@ -265,7 +248,7 @@ The *MoveIt ROS Control Controller Manager* can be configured by changing the ``
To test ROS controller integration with *MoveIt ROS Control Controller Manager*, launch the package generated by MSA by using the ``demo_gazebo.launch`` file. This will load your robot description, start the motion planning pipeline hosted in ``move_group`` node, and enable you to use the `Motion Planning Plugin <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_ in RViz to send goals to MoveIt, simulating the effect your ROS controllers will have on the real robot in Gazebo.

.. note::
Since the ``GripperActionController`` is not supported by MoveIt ROS Control Controller Manager, it can be replaced in the above example by a flavor of ``JointTrajectoryController`` supported by your hardware, for example:
Since the ``GripperActionController`` is not supported by the `MoveIt ROS Control Controller Manager`, it can be replaced in the above example by a flavor of ``JointTrajectoryController`` supported by your hardware, for example:

.. code-block:: yaml
Expand Down Expand Up @@ -312,14 +295,10 @@ The following headers declare the relevant classes and macros:

Two example *controller handle* implementations are included with MoveIt:

* `follow_joint_trajectory_controller_handle.h <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>`_

* See implementation in `follow_joint_trajectory_controller_handle.cpp <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp>`_
* `follow_joint_trajectory_controller_handle.h <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>`_ implemented in the `corresponding .cpp file <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp>`_

* `gripper_controller_handle.h <https://github.com/ros-planning/moveit/blob/master/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h>`_

* Implemented inline in the same header file

As you can see, writing a `controller handle <https://github.com/ros-planning/moveit/blob/master/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h#L104>`__ comes down to implementing:

* ``sendTrajectory`` method that translates `moveit_msgs::RobotTrajectory <http://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotTrajectory.html>`_ to a format the controller can understand
Expand Down Expand Up @@ -488,3 +467,19 @@ The following code listing demonstrates how to write a controller handle that tr
} // namespace example

Once implemented, the controller handle does not need to be exported, since it's returned by the controller handle allocator, which is exported.

Remapping ``/joint_states`` topic
---------------------------------
MoveIt requires joint states to be published on the ``/joint_states`` topic to internally maintain the robot's state.
If the joint states are published on another topic specific to your project, such as ``/robot/joint_states``, add a ``remap`` to the ``move_group`` node in ``move_group.launch`` file generated by MSA:

.. code-block:: XML
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<remap
from="joint_states"
to="robot/joint_states"
/>
<!-- Other settings -->
</node>

0 comments on commit b99848d

Please sign in to comment.