diff --git a/doc/controller_configuration/controller_configuration_tutorial.rst b/doc/controller_configuration/controller_configuration_tutorial.rst index e0e64a18e..35ef5041f 100644 --- a/doc/controller_configuration/controller_configuration_tutorial.rst +++ b/doc/controller_configuration/controller_configuration_tutorial.rst @@ -1,14 +1,63 @@ Low Level Controllers ===================== -MoveIt has access to many different controllers through a plugin interface inside the `MoveItControllerHandler `_ class. The MoveItControllerManager class is one of the options that is used to interact with a single ros_control node. MoveItControllerManager reads what controller(s) to use from a controllers.yaml file. +MoveIt executes planned trajectories by means of MoveIt controller managers. These are plugins that handle the interaction with low-level controllers. +There are different options available: -Here we will walk through configuring MoveIt with the controllers on your robot. We will assume that your robot offers a ``FollowJointTrajectory`` action service for the arms on your robot and (optionally) a ``GripperCommand`` service for your gripper. If your robot does not offer this we recommend the `ROS control `_ framework for easily adding this functionality around your hardware communication layer. +* `Simple Controller Manager`_ allows to interface any low-level controller supporting either the `JointTrajectoryController `_ or `GripperActionController `_ API from the `ROS controllers `_ package +* `ROS Control Controller Manager`_ and `Multi Controller Manager`_ allow to interface `ROS Control `_ controllers that support the `FollowJointTrajectory action `_ without further configuration. This manager directly interacts with the corresponding `ROS Controller Manager `_. +* `Fake Controller Manager`_ provides fake trajectory execution w/o a real robot. It is used for visualization in RViz and can be configured to use different methods for interpolation between waypoints. +* ROS controllers that don't support the `FollowJointTrajectory action `_ can be used with a ROS Control Controller Manager by implementing integration interfaces satisfying the `ControllerHandle `_ API. +* Completely custom controllers can be integrated by writing and using your own controller manager plugin (not recommended). -YAML Configuration ------------------- -The ``controllers.yaml`` configuration file is located in the ``robot_moveit_config/config`` directory of your MoveIt robot config package. This specifies the controller configuration for your robot. Here's an example file for configuring a ``FollowJointTrajectory`` action controller for the ``panda_arm`` and a ``GripperCommand`` gripper controller for its ``hand``: :: +In the following sections, we will look at each of these options in more detail. - controller_list: +Simple Controller Manager +------------------------- + +The `JointTrajectoryController `_ and `GripperActionController `_ from the `ROS controllers `_ package are supported out of the box and can be easily configured using the `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_ (*MSA*) on the *Controllers* page. + +Controller Mapping +^^^^^^^^^^^^^^^^^^ + +The `ROS Controller Manager `_ loads these controllers `as plugins `_ via their ``type`` name configured in the ``ros_controllers.yaml`` file generated by MSA: + +.. code-block:: yaml + + panda_arm_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + gains: + shoulder_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + upperarm_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + forearm_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + gripper_controller: + type: position_controllers/GripperActionController + joint: panda_finger_joint1 + +The ``MoveItSimpleControllerManager`` configured by MSA `as the default `_ MoveIt controller manager will bridge these controllers with MoveIt pipeline by reading the ``simple_moveit_controllers.yaml`` configuration file, for example: + +.. code-block:: yaml + + controller_list: - name: panda_arm_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory @@ -21,7 +70,7 @@ The ``controllers.yaml`` configuration file is located in the ``robot_moveit_con - panda_joint5 - panda_joint6 - panda_joint7 - - name: hand_controller + - name: gripper_controller action_ns: gripper_action type: GripperCommand default: true @@ -30,146 +79,407 @@ The ``controllers.yaml`` configuration file is located in the ``robot_moveit_con - panda_finger_joint1 - panda_finger_joint2 -There are many different parameters that can be used for different types of controllers. +The mapping from ROS controller ``name`` to a ``MoveItControllerHandle`` ``type`` is done by using the pre-defined *integration types* ``FollowJointTrajectory`` and ``GripperCommand``, which can interface any controller implementing the corresponding action interface (i.e. not only `ROS Control `_ controllers). + +The ``action_ns`` setting specifies the *action server* topic exposed by the ROS controller. The full topic name is ``/``. +If you were to list topics by using ``rostopic list`` with the above two ROS controllers loaded, you would see something like the following: :: + +/panda_arm_controller/follow_joint_trajectory/goal +/panda_arm_controller/follow_joint_trajectory/feedback +/panda_arm_controller/follow_joint_trajectory/result +/gripper_controller/gripper_action/goal +/gripper_controller/gripper_action/feedback +/gripper_controller/gripper_action/result + +There are many different parameters that can be defined for the two types of simple controller interfaces. FollowJointTrajectory Controller Interface ------------------------------------------- -The parameters are: - * *name*: The name of the controller. (See debugging information below for important notes). - * *action_ns*: The action namespace for the controller. (See debugging information below for important notes). - * *type*: The type of action being used (here FollowJointTrajectory). - * *default*: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints. - * *joints*: Names of all the joints that are being addressed by this interface. +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + * ``name``: The name of the controller. (See debugging information below for important notes). + * ``action_ns``: The action namespace for the controller. (See debugging information below for important notes). + * ``type``: The type of action being used (here FollowJointTrajectory). + * ``default``: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints. + This is useful when additional controllers are defined for the same set of joints: + + * One such scenario is using the `Motion Planning RViz Plugin with a joystick <../joystick_control_teleoperation/joystick_control_teleoperation_tutorial.html?highlight=joystick>`_. In this case, ``JointGroupVelocityController`` or ``JointGroupPositionController`` could be configured in ``ros_controllers.yaml`` for the same set of joints. + * Another scenario is configuring the robot for use with `MoveIt Servo <../realtime_servo/realtime_servo_tutorial.html>`_ which lets you control the robot by using a joystick or a `SpaceMouse `_. MoveIt Servo supports ``trajectory_msgs/JointTrajectory`` and ``std_msgs/Float64MultiArray`` so a ``JointGroupVelocityController`` or ``JointGroupPositionController`` could be configured as well. + + * ``joints``: Names of all the joints that are being addressed by this interface. GripperCommand Controller Interface ------------------------------------ -The parameters are: - * *name*: The name of the controller. (See debugging information below for important notes). - * *action_ns*: The action namespace for the controller. (See debugging information below for important notes). - * *type*: The type of action being used (here GripperCommand). - * *default*: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints. - * *joints*: Names of all the joints that are being addressed by this interface. - * *command_joint*: The single joint, controlling the actual state of the gripper. This is the only value that is sent to the controller. Has to be one of the joints above. If not specified, the first entry in *joints* will be used instead. - * *parallel*: When this is set, *joints* should be of size 2, and the command will be the sum of the two joints. +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + * ``name``: The name of the controller. (See debugging information below for important notes). + * ``action_ns``: The action namespace for the controller. (See debugging information below for important notes). + * ``type``: The type of action being used (here GripperCommand). + * ``default``: The default controller is the primary controller chosen by MoveIt for communicating with a particular set of joints. + * ``joints``: Names of all the joints that are being addressed by this interface. + * ``command_joint``: The single joint, controlling the actual state of the gripper. This is the only value that is sent to the controller. Has to be one of the joints above. If not specified, the first entry in *joints* will be used instead. + * ``parallel``: When this is set, *joints* should be of size 2, and the command will be the sum of the two joints. Optional Allowed Trajectory Execution Duration Parameters ---------------------------------------------------------- +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +For each controller it is optionally possible to set the ``allowed_execution_duration_scaling`` and ``allowed_goal_duration_margin`` parameters. These are controller-specific overrides of the global values ``trajectory_execution/allowed_execution_duration_scaling`` and ``trajectory_execution/allowed_goal_duration_margin``. As opposed to the global values, the contoller-specific ones cannot be dynamically reconfigured at runtime. The parameters are used to compute the allowed trajectory execution duration by scaling the expected execution duration and adding the margin afterwards. If this duration is exceeded the trajectory will be cancelled. -For each controller it is optionally possible to set the *allowed_execution_duration_scaling* and *allowed_goal_duration_margin* parameters. These are controller-specific overrides of the global values *trajectory_execution/allowed_execution_duration_scaling* and *trajectory_execution/allowed_goal_duration_margin*. As opposed to the global values, the contoller-specific ones cannot be dynamically reconfigured at runtime. The parameters are used to compute the allowed trajectory execution duration by scaling the expected execution duration and adding the margin afterwards. If this duration is exceeded the trajectory will be cancelled. The controller-specific parameters can be set as follows :: +Trajectory execution parameters can be configured to fine-tune the allowed trajectory execution duration, overriding the global settings ``trajectory_execution/allowed_execution_duration_scaling`` and ``trajectory_execution/allowed_goal_duration_margin``, where the former scales the allowed execution duration by a given factor and the latter allows for a fixed (duration-independent) margin (applied after scaling). If the execution does not finish within the specified margins, execution will be canceled. - controller_list: - - name: arm_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 +Additional options for tuning the behavior and safety checks of MoveIt's execution pipeline can be configured in ``trajectory_execution.launch.xml`` file generated by MSA: -Create the Controller launch file ---------------------------------- -Now, create the controller launch file (call it ``robot_moveit_controller_manager.launch.xml`` where ``robot`` is the name of your robot as specified when you created your MoveIt robot config package). +* ``execution_duration_monitoring``: when ``false``, will not throw error if a controller takes longer than expected to complete a trajectory. +* ``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. -Add the following lines to this file: :: +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 `_. Instead of using a bridging configuration file like ``simple_moveit_controllers.yaml``, this controller manager directly queries the ROS Controller Manager for available controllers. -MAKE SURE to replace ``robot_moveit_config`` with the correct name of your MoveIt robot config package. +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*. -Now, you should be ready to have MoveIt talk to your robot. +Multi Controller Manager +------------------------ -Debugging Information ---------------------- -The ``FollowJointTrajectory`` or ``GripperCommand`` interfaces on your robot must be communicating in the namespace: ``/name/action_ns``. In the above example, you should be able to see the following topics (using *rostopic list*) on your robot: +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 ``/``). - * /panda_arm_controller/follow_joint_trajectory/goal - * /panda_arm_controller/follow_joint_trajectory/feedback - * /panda_arm_controller/follow_joint_trajectory/result - * /hand_controller/gripper_action/goal - * /hand_controller/gripper_action/feedback - * /hand_controller/gripper_action/result +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``: -You should also be able to see (using ``rostopic info topic_name``) that the topics are published/subscribed to by the controllers on your robot and also by the **move_group** node. +.. code-block:: XML -Remapping /joint_states topic ------------------------------ + -When you run a `move group node <../move_group_interface/move_group_interface_tutorial.html>`_, you may need to remap the topic /joint_states to /robot/joint_states, otherwise MoveIt won't have feedback from the joints. To do this remapping you could make a simple launch file for your node as follows: :: +.. 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 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Or you can make a subscriber with the correct topic name and then ensure that the starting robot state for your move group corresponds to a correct joints angle by using the call back of this subscriber. +All controller names get prefixed by the namespace of their `ROS Control `_ node. For this reason, controller names should not contain slashes. -Trajectory Execution Manager Options ------------------------------------- +For a particular `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. -There are several options for tuning the behavior and safety checks of the execution pipeline in MoveIt. In your ``moveit_config`` package edit the ``trajectory_execution.launch.xml`` file to change the following parameters: +Fake Controller Manager +----------------------- - - ``execution_duration_monitoring``: when false, will not throw error is trajectory takes longer than expected to complete at the low-level controller side - - ``allowed_goal_duration_margin``: Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) - - ``allowed_start_tolerance``: Allowed joint-value tolerance for validation that trajectory's first point matches current robot state. If set to zero will skip waiting for robot to stop after execution +MoveIt comes with a series of fake trajectory controllers that can be used for simulations. For example, the ``demo.launch`` file generated by MSA employs fake controllers for nice visualization in RViz. + +The configuration for these controllers is stored in ``fake_controllers.yaml`` also generated by MSA, for example: + +.. code-block:: yaml + + controller_list: + - name: fake_arm_controller + type: $(arg fake_execution_type) + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + - name: fake_gripper_controller + type: $(arg fake_execution_type) + joints: + - panda_finger_joint1 + - panda_finger_joint2 + initial: # Define initial robot poses per group + - group: panda_arm + pose: ready + - group: panda_hand + pose: open + +The ``type`` setting specifies the *fake controller interpolation type*: + +* ``interpolate``: performs smooth interpolation between trajectory waypoints - the default for visualization. +* ``via points``: jumps to the position specified by each trajectory waypoint without interpolation in between - useful for visual debugging. +* ``last point``: warps directly to the last trajectory waypoint - the fastest method for off-line benchmarking and unit tests. + +Integrating Controllers +----------------------- -Example Controller Manager --------------------------- +MoveIt Controller Managers only support controllers that implement the ``FollowJointTrajectory`` action out of the box. -MoveIt controller managers, somewhat a misnomer, are the interfaces to your custom low level controllers. A better way to think of them are *controller interfaces*. For most use cases, the included :moveit_codedir:`MoveItSimpleControllerManager ` is sufficient if your robot controllers already provide ROS actions for FollowJointTrajectory. If you use *ros_control*, the included :moveit_codedir:`MoveItRosControlInterface ` is also ideal. +This is because only a `ControllerHandleAllocator `_ for this action type is `exported `_ as a plugin. Even though there is a `ControllerHandle `__ 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. -However, for some applications you might desire a more custom controller manager. An example template for starting your custom controller manager is provided :codedir:`here `. +ROS Controllers with a ``FollowJointTrajectory`` action interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Fake Controller Manager ------------------------ +*Controller handles* implemented by MoveIt bridge ROS Controllers with the MoveIt motion planning pipeline by means of an `Action Client `_, as long as the controller starts an *Action Server* that handles one of the two types of supported action interfaces: -MoveIt comes with a series of fake trajectory controllers that can be used for simulations. -For example, the ``demo.launch`` generated by MoveIt's setup assistant, employs fake controllers for nice visualization in RViz. -For configuration, edit the file ``config/fake_controllers.yaml``, and adjust the desired controller type. -The following controllers are available: +* The `Joint Trajectory Controller Handle `_ can be used for controllers that support the `Follow Joint Trajectory Action `_. +* The `Gripper Controller Handle `_ can be used for controllers that support the `Gripper Command Action `_. -* **interpolate**: perform smooth interpolation between via points - the default for visualization -* **via points**: traverse via points, w/o interpolation in between - useful for visual debugging -* **last point**: warp directly to the last point of the trajectory - fastest method for off-line benchmarking +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. -Fake Controller Yaml File -------------------------- +For example, see the stock Joint Trajectory Controller `plugin registration `_, which links several flavors of this controller exported from ``ros_controllers`` package with the corresponding MoveIt Controller Handle that supports `Follow Joint Trajectory Action `_ via an exported MoveIt *Controller Handle Allocator* plugin. -.. code:: yaml +The same pattern can be followed to link any other ROS controller that exposes a `Follow Joint Trajectory Action `_ server with an existing MoveIt *Controller Handle* so that it can receive trajectory commands. - fake_interpolating_controller_rate: 10 - controller_list: - - name: fake_arm_controller - type: interpolate | via points | last point - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller - joints: - [] +First, create a plugin description file: -In order to load an initial pose, one can have a list of (group, pose) pairs as follows: +.. code-block:: XML -.. code:: yaml + + + + Controller description + + + - initial: - - group: arm - pose: ready +Reference this plugin description file in your package.xml's ``export`` section: -Controller Switching and Namespaces ------------------------------------ +.. code-block:: XML -All controller names get prefixed by the namespace of their ros_control node. For this reason, controller names should not contain slashes, and can't be named ``/``. For a particular node, MoveIt can decide which controllers to have started or stopped. Since only controller names with registered allocator plugins are handled over MoveIt, MoveIt takes care of stopping controllers based on their claimed resources if a to-be-started controller needs any of those resources. + + + -Controllers for Multiple Nodes ------------------------------- +After building the package, any controllers in ``ros_controllers.yaml`` that reference ``controller_package_name/controller_type_name`` will become available for use with MoveIt. + +The *MoveIt ROS Control Controller Manager* can be configured by changing the ``moveit_controller_manager`` setting to ``ros_control``. The MoveIt configuration package auto-generated by MSA includes the `demo_gazebo.launch `_ file that already configures this manager type in addition to launching `Gazebo `_ simulation and visualizing the robot state in `RViz `_. + +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 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 + + gripper_controller: + type: position_controllers/JointTrajectoryController + joints: + - gripper + gains: + gripper: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + +ROS Controllers with another interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -MoveItMultiControllerManager can be used for more than one ros_control nodes. It works by creating several MoveItControllerManagers, one for each node. It instantiates them with their respective namespace and takes care of proper delegation. To use it must be added to the launch file. :: +What if you need to use a ROS controller that does not support `Follow Joint Trajectory Action `_ with *MoveIt ROS Control Controller Manager*? Some examples from `ROS controllers `_ package include: - +* `Gripper Action Controller `_ discussed earlier +* `Joint Position and Joint Group Position `_ Controllers +* `Joint Position, Joint Velocity, and Joint Group Velocity `_ Controllers +* `Joint Position, Joint Velocity, Joint Effort, Joint Group Effort, and Joint Group Position `_ Controllers + +In this case, a *Controller Handle* and a *Controller Handle Allocator* may need to be implemented. The allocator will also need to be exported by your package as a plugin. + +The following package dependencies are required for implementing controller handles and allocators: + +* ``moveit_ros_control_interface`` - Provides base classes for controller handles and allocators. +* ``pluginlib`` - Provides macros for exporting a class as a plugin, only needed to export the controller handle allocator. + +The ``actionlib`` package may also be needed for communicating with the ROS controller via an *Action Client* if it exposes an *Action Server*. + +The following headers declare the relevant classes and macros: + +* ``#include `` + + * declares ``moveit_controller_manager::MoveItControllerHandle`` class + * declares ``moveit_ros_control_interface::ControllerHandleAllocator`` class + +* ``#include `` + + * declares ``PLUGINLIB_EXPORT_CLASS`` macro for exporting plugins + +Two example *controller handle* implementations are included with MoveIt: + +* `follow_joint_trajectory_controller_handle.h `_ implemented in the `corresponding .cpp file `_ + +* `gripper_controller_handle.h `_ + +As you can see, writing a `controller handle `__ comes down to implementing: + +* ``sendTrajectory`` method that translates `moveit_msgs::RobotTrajectory `_ to a format the controller can understand +* ``cancelExecution`` method to tell the controller to stop any active trajectories +* ``waitForExecution`` method that will block the calling thread until the controller finishes or the ``timeout`` is reached +* ``getLastExecutionStatus`` method that returns the status of the last requested trajectory. + +One example *controller handle allocator* plugin implementation is included with MoveIt: + +* `joint_trajectory_controller_plugin.cpp `_ + +The only job of a controller handle allocator is to create a new instance of the controller handle. The following example implements an allocator for a custom controller handle of type ``example::controller_handle_example`` (the code listing for this class is provided in the following section): + +.. code-block:: c++ + + // declares example::controller_handle_example class + #include "controller_handle_example.h" + #include + #include + + namespace example + { + class controller_handle_allocator_example : public moveit_ros_control_interface::ControllerHandleAllocator + { + public: + moveit_controller_manager::MoveItControllerHandlePtr alloc(const std::string& name, + const std::vector& resources) override + { + return std::make_shared(name); + } + }; + } // namespace example + + PLUGINLIB_EXPORT_CLASS(example::controller_handle_allocator_example, + moveit_ros_control_interface::ControllerHandleAllocator); + + +This example controller handle allocator can be exported by creating a plugin definition file which is then referenced in the ``exports`` section of ``package.xml``: + +.. code-block:: XML + + + + + Example Controller Handle Allocator for MoveIt! + + + + +This plugin definition links the name of a controller you are integrating with MoveIt (specified by the ``name`` attribute) with the type of the allocator you implemented (specified by the ``type`` attribute), such as the one in the example above. + +The ``base_class_type`` must be set to ``moveit_ros_control_interface::ControllerHandleAllocator`` to make the allocator discoverable by MoveIt. + +The plugin definition can then be referenced in the package manifest: + +.. code-block:: XML + + + + + + +The translation between `moveit_msgs::RobotTrajectory `_ message and the type of command supported by the controller would be done by implementing a controller handle. + +The following code listing demonstrates how to write a controller handle that translates robot trajectory message into joint trajectory message. Of course this is already taken care of by existing controller handles included with MoveIt, so you would substitute the message that your custom controller understands here instead of ``FollowJointTrajectory``: + +.. code-block:: c++ + + #include + #include + #include + #include + + namespace example + { + class controller_handle_example : public moveit_controller_manager::MoveItControllerHandle + { + private: + // Idle or done executing trajectory + bool done_; + + // Connects to Action Server exposed by the controller + std::shared_ptr> actionClient_; + + public: + controller_handle_example(const std::string& name) + { + std::string actionName = name + "/follow_joint_trajectory"; + + actionClient_ = + std::make_shared>(actionName, true); + + // Timeout can be loaded from settings + actionClient_->waitForServer(ros::Duration(20.0)); + + if (!actionClient_->isServerConnected()) + { + // Report connection error + actionClient_.reset(); + } + } + + public: + // MoveIt calls this method when it wants to send a trajectory goal to execute + bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override + { + if (!actionClient_) + { + // Report connection error + return false; + } + + control_msgs::FollowJointTrajectoryGoal goal; + goal.trajectory = trajectory.joint_trajectory; + + actionClient_->sendGoal( + goal, + [this](const auto& state, const auto& result) { + // Complete trajectory callback + done_ = true; + }, + [this] { + // Begin trajectory callback + }, + [this](const auto& feedback) { + // Trajectory state callback + }); + + done_ = false; + + return true; + } + + // MoveIt calls this method when it wants a blocking call until done + bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override + { + if (actionClient_ && !done_) + return actionClient_->waitForResult(ros::Duration(5.0)); + + return true; + } + + // MoveIt calls this method to get status updates + moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override + { + // Report last status here + return moveit_controller_manager::ExecutionStatus::SUCCEEDED; + } + + // MoveIt calls this method to abort trajectory goal execution + bool cancelExecution() override + { + if (!actionClient_) + return false; + + actionClient_->cancelGoal(); + done_ = true; + + return true; + } + }; + } // 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 + + + + + +