diff --git a/README.md b/README.md index e2022ed19..0bd3d3fb9 100644 --- a/README.md +++ b/README.md @@ -92,7 +92,7 @@ The robot is basically a box moving according to differential drive kinematics. The example shows how to implement robot hardware with separate communication to each actuator. -##### Example 8 +##### Example 8: Using transmissions *RRBot* - or ''Revolute-Revolute Manipulator Robot'' - with an exposed transmission interface @@ -268,142 +268,3 @@ Available launch file options: ### Example 1-Sim: "Industrial Robots with only one interface" (Gazebo simulation) - **TBA** - - - - -## Controllers and moving hardware - -To move the robot you should load and start controllers. -The `JointStateBroadcaster` is used to publish the joint states to ROS topics. -Direct joint commands are sent to this robot via the `ForwardCommandController` and `JointTrajectoryController`. -The sections below describe their usage. -Check the [Results](##result) section on how to ensure that things went well. - -**NOTE**: Before doing any action with controllers check their state using command: -``` -ros2 control list_controllers -``` - - -### JointStateBroadcaster - -Open another terminal and load, configure and start `joint_state_broadcaster`: -``` -ros2 control set_controller_state joint_state_broadcaster start -``` -Check if controller is loaded properly: -``` -ros2 control list_controllers -``` -You should get the response: -``` -joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active -``` - -Now you should also see the *RRbot* represented correctly in `RViz`. - - -### Using ForwardCommandController - -1. If you want to test hardware with `ForwardCommandController` first load a controller (not always needed): - ``` - ros2 control load_controller forward_position_controller - ``` - Check if the controller is loaded properly: - ``` - ros2 control list_controllers - ``` - -2. Then configure it: - ``` - ros2 control set_controller_state forward_position_controller configure - ``` - Check if the controller is loaded properly: - ``` - ros2 control list_controllers - ``` - You should get the response: - ``` - forward_position_controller[forward_command_controller/ForwardCommandController] inactive - ``` - -3. Now start the controller: - ``` - ros2 control switch_controllers --start forward_position_controller - ``` - Check if controllers are activated: - ``` - ros2 control list_controllers - ``` - You should get `active` in the response: - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - forward_position_controller[forward_command_controller/ForwardCommandController] active - ``` - -4. Send a command to the controller, either: - - a. Manually using ros2 cli interface: - ``` - ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: - - 0.5 - - 0.5" - ``` - B. Or you can start a demo node which sends two goals every 5 seconds in a loop: - ``` - ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py - ``` - You can adjust the goals in [rrbot_forward_position_publisher.yaml](ros2_control_demo_bringup/config/rrbot_forward_position_publisher.yaml). - -### Using JointTrajectoryController - -1. If you want to test hardware with `JointTrajectoryController` first load and configure a controller (not always needed): - ``` - ros2 control load_controller position_trajectory_controller --set-state configure - ``` - Check if the controller is loaded and configured properly: - ``` - ros2 control list_controllers - ``` - You should get the response: - ``` - position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive - ``` - -2. Now start the controller (and stop other running contorller): - ``` - ros2 control switch_controllers --stop forward_position_controller --start position_trajectory_controller - ``` - Check if controllers are activated: - ``` - ros2 control list_controllers - ``` - You should get `active` in the response: - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active - ``` - -3. Send a command to the controller using demo node which sends four goals every 6 seconds in a loop: - ``` - ros2 launch ros2_control_demo_bringup test_joint_trajectory_controller.launch.py - ``` - You can adjust the goals in [rrbot_joint_trajectory_publisher.yaml](ros2_control_demo_bringup/config/rrbot_joint_trajectory_publisher.yaml). - -## Result - -1. Independently from the controller you should see how the example's output changes. - Look for the following lines - ``` - [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0! - [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1! - ``` - -2. If you echo the `/joint_states` or `/dynamic_joint_states` topics you should also get similar values. - ``` - ros2 topic echo /joint_states - ros2 topic echo /dynamic_joint_states - ``` - -3. You should also see the *RRbot* moving in `RViz`. diff --git a/doc/index.rst b/doc/index.rst deleted file mode 100644 index 6dbc729e1..000000000 --- a/doc/index.rst +++ /dev/null @@ -1,110 +0,0 @@ -.. _ros2_control_demos: - -Demos ------ - -This repository provides templates for the development of ros2_control-enabled robots and a simple simulations to demonstrate and prove ros2_control concepts. - -Repository organization -^^^^^^^^^^^^^^^^^^^^^^^ - -**Note:** the following list is comprehensive. - -The repository is organized in the following packages: - - - ``ros2_control_demo_hardware`` - implementation of example hardware interfaces, - - ``ros2_control_demo_bringup`` - nodes starting hardware interfaces, controllers and GUIs. - - ``ros2_control_test_node`` - nodes for testing ros2_control-enabled robots and their integration in the framework. - -Mode switching demo -^^^^^^^^^^^^^^^^^^^ - -Start up the multi interface rrbot system: - -.. code-block:: bash - - ros2 launch ros2_control_demo_bringup rrbot_system_multi_interface.launch.py - -List the available interfaces - -.. code-block:: bash - - $ ros2 control list_hardware_interfaces - command interfaces - joint1/acceleration [unclaimed] - joint1/position [unclaimed] - joint1/velocity [unclaimed] - joint2/acceleration [unclaimed] - joint2/position [unclaimed] - joint2/velocity [unclaimed] - state interfaces - joint1/acceleration - joint1/position - joint1/velocity - joint2/acceleration - joint2/position - joint2/velocity - -Load and configure all controllers - -.. code-block:: bash - - ros2 control load_controller forward_position_controller --set-state configure - ros2 control load_controller forward_velocity_controller --set-state configure - ros2 control load_controller forward_acceleration_controller --set-state configure - ros2 control load_controller forward_illegal1_controller --set-state configure - ros2 control load_controller forward_illegal2_controller --set-state configure - ros2 control load_controller joint_state_broadcaster --set-state configure - - -Start the position controller - -.. code-block:: bash - - ros2 control set_controller_state forward_position_controller start - -Check the hardware interfaces, position interfaces should be claimed now - -.. code-block:: bash - - $ ros2 control list_hardware_interfaces - command interfaces - joint1/acceleration [unclaimed] - joint1/position [claimed] - joint1/velocity [unclaimed] - joint2/acceleration [unclaimed] - joint2/position [claimed] - joint2/velocity [unclaimed] - state interfaces - joint1/acceleration - joint1/position - joint1/velocity - joint2/acceleration - joint2/position - joint2/velocity - -Let's switch controllers now to velocity - -.. code-block:: bash - - ros2 control switch_controllers --stop forward_position_controller --start forward_velocity_controller - -List hardware interfaces again to see that indeed position interfaces have been unclaimed while velocity is claimed now - -.. code-block:: bash - - $ ros2 control list_hardware_interfaces - command interfaces - joint1/acceleration [unclaimed] - joint1/position [unclaimed] - joint1/velocity [claimed] - joint2/acceleration [unclaimed] - joint2/position [unclaimed] - joint2/velocity [claimed] - state interfaces - joint1/acceleration - joint1/position - joint1/velocity - joint2/acceleration - joint2/position - joint2/velocity diff --git a/example_1/README.rst b/example_1/README.rst index 1ba724026..438cc82a5 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -69,7 +69,7 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. .. code-block:: shell - ros2 topic pub /position_commands std_msgs/msg/Float64MultiArray "data: + ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: - 0.5 - 0.5" @@ -87,6 +87,100 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + If you echo the ``/joint_states`` or ``/dynamic_joint_states`` topics you should now get similar values, namely the simulated states of the robot + + .. code-block:: shell + + ros2 topic echo /joint_states + ros2 topic echo /dynamic_joint_states + +6. Let's switch to a different controller, the ``Joint Trajectory Controller``. + Load the controller manually by + + .. code-block:: shell + + ros2 control load_controller position_trajectory_controller + + what should return ``Successfully loaded controller position_trajectory_controller``. Check the status + + .. code-block:: shell + + ros2 control list_controllers + + what shows you that the controller is loaded but unconfigured. + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] unconfigured + + Configure the controller by setting it ``inactive`` by + + .. code-block:: shell + + ros2 control set_controller_state position_trajectory_controller inactive + + what should give ``Successfully configured position_trajectory_controller``. + Note that the parameters are already set in `rrbot_controllers.yaml `__ + but the controller was not loaded from the `launch file rrbot.launch.py `__ before. + + As an alternative, you can load the controller directly in ``inactive``-state by means of the option for ``load_controller`` + + .. code-block:: shell + + ros2 control load_controller position_trajectory_controller --set-state inactive + + You should get the result ``Successfully loaded controller position_trajectory_controller into state inactive``. + + See if it loaded properly with + + .. code-block:: shell + + ros2 control list_controllers + + what should now return + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive + + Note that the controller is loaded but still ``inactive``. Now you can switch the controller by + + .. code-block:: shell + + ros2 control set_controller_state forward_position_controller inactive + ros2 control set_controller_state position_trajectory_controller active + + or simply via this one-line command + + .. code-block:: shell + + ros2 control switch_controllers --activate position_trajectory_controller --deactivate forward_position_controller + + Again, check via + + .. code-block:: shell + + ros2 control list_controllers + + what should now return + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] inactive + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active + + Send a command to the controller using demo node, which sends four goals every 6 seconds in a loop: + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_1 test_joint_trajectory_controller.launch.py + + You can adjust the goals in `rrbot_joint_trajectory_publisher `__. Files used for this demos ######################### @@ -99,6 +193,10 @@ Files used for this demos + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ - RViz configuration: `rrbot.rviz `__ +- Test nodes goals configuration: + + + `rrbot_forward_position_publisher `__ + + `rrbot_joint_trajectory_publisher `__ - Hardware interface plugin: `rrbot.cpp `__ @@ -107,3 +205,4 @@ Controllers from this demo ########################## - ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ - ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ +- ``Joint Trajectory Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/ros2_control_demo_bringup/config/rrbot_joint_trajectory_publisher.yaml b/example_1/bringup/config/rrbot_joint_trajectory_publisher.yaml similarity index 100% rename from ros2_control_demo_bringup/config/rrbot_joint_trajectory_publisher.yaml rename to example_1/bringup/config/rrbot_joint_trajectory_publisher.yaml diff --git a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py b/example_1/bringup/launch/test_joint_trajectory_controller.launch.py similarity index 91% rename from ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py rename to example_1/bringup/launch/test_joint_trajectory_controller.launch.py index 3e6f2fe8a..1ece95065 100644 --- a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py +++ b/example_1/bringup/launch/test_joint_trajectory_controller.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): position_goals = PathJoinSubstitution( [ - FindPackageShare("ros2_control_demo_bringup"), + FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot_joint_trajectory_publisher.yaml", ] @@ -31,7 +31,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", parameters=[position_goals], diff --git a/example_1/description/rviz/rrbot.rviz b/example_1/description/rviz/rrbot.rviz index 0efbecc38..8e78cd635 100644 --- a/example_1/description/rviz/rrbot.rviz +++ b/example_1/description/rviz/rrbot.rviz @@ -103,24 +103,6 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 1 - Arrow Width: 0.5 - Class: rviz_default_plugins/Wrench - Enabled: true - Force Arrow Scale: 0.20000000298023224 - Force Color: 204; 51; 51 - History Length: 1 - Name: Wrench - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /fts_broadcaster/wrench - Torque Arrow Scale: 0.20000000298023224 - Torque Color: 204; 204; 51 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/example_1/package.xml b/example_1/package.xml index b42c85c92..5da5ebfdc 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -19,6 +19,8 @@ rclcpp rclcpp_lifecycle + ros2_controllers_test_nodes + ament_cmake_gtest