diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index b1a543f0c..4b64730e2 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -34,6 +34,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_11 ros2_control_demo_example_12 vcs-repo-file-url: | diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 9c6607bb7..2eb79f36f 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -29,6 +29,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_11 ros2_control_demo_example_12 ament_lint_100: diff --git a/doc/index.rst b/doc/index.rst index c3af543bf..20eafd07a 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -67,6 +67,9 @@ Example 8: "Using transmissions" Example 9: "Gazebo Classic" Demonstrates how to switch between simulation and hardware. +Example 11: "CarlikeBot" + *CarlikeBot* with an Ackermann steering controller would be similiar to how a car would be controlled. + Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. @@ -259,4 +262,5 @@ Examples Example 7: Full tutorial with a 6DOF robot <../example_7/doc/userdoc.rst> Example 8: Using transmissions <../example_8/doc/userdoc.rst> Example 9: Gazebo classic <../example_9/doc/userdoc.rst> + Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 7d7d899d6..10cc6d5d6 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -14,7 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 3efe5621e..39ad3dbd8 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -1,29 +1,30 @@ :github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_11/doc/userdoc.rst -.. _ros2_control_demos_example_2_userdoc: +.. _ros2_control_demos_example_11_userdoc: ********* -DiffBot +CarlikeBot ********* -*DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. -The robot is basically a box moving according to differential drive kinematics. +*CarlikeBot* is a simple mobile base with ackermann drive. -For *example_2*, the hardware interface plugin is implemented having only one interface. +This example shows how to use the ackermann steering controller, which is a sub-design of the steering controller library. * The communication is done using proprietary API to communicate with the robot control box. * Data for all joints is exchanged at once. -The *DiffBot* URDF files can be found in ``description/urdf`` folder. +The *CarlikeBot* URDF files can be found in ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst Tutorial steps -------------------------- -1. To check that *DiffBot* description is working properly use following launch commands +1. To check that *CarlikeBot* description is working properly use following launch commands .. code-block:: shell - ros2 launch ros2_control_demo_example_2 view_robot.launch.py + ros2 launch ros2_control_demo_example_11 view_robot.launch.py .. warning:: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. @@ -31,20 +32,20 @@ Tutorial steps .. image:: carlikebot.png :width: 400 - :alt: Carlike Mobile Robot + :alt: Ackermann Mobile Robot -2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with +2. To start *CarlikeBot* example open a terminal, source your ROS2-workspace and execute its launch file with .. code-block:: shell - ros2 launch ros2_control_demo_example_2 diffbot.launch.py + ros2 launch ros2_control_demo_example_11 carlikebot.launch.py The launch file loads and starts the robot hardware, controllers and opens *RViz*. In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. If you can see an orange box in *RViz* everything has started properly. - Still, to be sure, let's introspect the control system before moving *DiffBot*. + Still, to be sure, let's introspect the control system before moving *CarlikeBot*. 3. Check if the hardware interface loaded properly, by opening another terminal and executing @@ -57,15 +58,21 @@ Tutorial steps .. code-block:: shell command interfaces - left_wheel_joint/velocity [available] [claimed] - right_wheel_joint/velocity [available] [claimed] + ackermann_steering_controller/angular/position [unavailable] [unclaimed] + ackermann_steering_controller/linear/velocity [unavailable] [unclaimed] + left_steering_joint/position [available] [claimed] + rear_left_wheel_joint/velocity [available] [claimed] + rear_right_wheel_joint/velocity [available] [claimed] + right_steering_joint/position [available] [claimed] state interfaces - left_wheel_joint/position - left_wheel_joint/velocity - right_wheel_joint/position - right_wheel_joint/velocity + left_steering_joint/position + rear_left_wheel_joint/position + rear_left_wheel_joint/velocity + rear_right_wheel_joint/position + rear_right_wheel_joint/velocity + right_steering_joint/position - The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. + The ``[claimed]`` marker on command interfaces means that a controller has access to command *CarlikeBot*. 4. Check if controllers are running @@ -77,47 +84,49 @@ Tutorial steps .. code-block:: shell - diffbot_base_controller[diff_drive_controller/DiffDriveController] active - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + ackermann_steering_controller[ackermann_steering_controller/AckermannSteeringController] active 5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: .. code-block:: shell - ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: + ros2 topic pub --rate 30 /ackermann_steering_controller/reference_unstamped geometry_msgs/msg/Twist "linear: x: 0.7 y: 0.0 z: 0.0 - angular: + angular: x: 0.0 y: 0.0 - z: 1.0" + z: 0.01" You should now see an orange box circling in *RViz*. Also, you should see changing states in the terminal where launch file is started. .. code-block:: shell - [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! - [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'! + [CarlikeBotSystemHardware]: Got command 0.00464 for 'left_steering_joint' + [CarlikeBotSystemHardware]: Got command 0.00465 for 'right_steering_joint' + [CarlikeBotSystemHardware]: Got command 13.97230 for 'rear_left_wheel_joint' + [CarlikeBotSystemHardware]: Got command 14.02830 for 'rear_right_wheel_joint' Files used for this demos -------------------------- -* Launch file: `diffbot.launch.py `__ -* Controllers yaml: `diffbot_controllers.yaml `__ -* URDF file: `diffbot.urdf.xacro `__ +* Launch file: `carlikebot.launch.py `__ +* Controllers yaml: `carlikebot_controllers.yaml `__ +* URDF file: `carlikebot.urdf.xacro `__ - * Description: `diffbot_description.urdf.xacro `__ - * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + * Description: `carlikebot_description.urdf.xacro `__ + * ``ros2_control`` tag: `carlikebot.ros2_control.xacro `__ -* RViz configuration: `diffbot.rviz `__ +* RViz configuration: `carlikebot.rviz `__ -* Hardware interface plugin: `diffbot_system.cpp `__ +* Hardware interface plugin: `carlikebot_system.cpp `__ Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -* ``Diff Drive Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Ackermann Steering Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index f728ec181..7303e1390 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -301,7 +301,7 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH hw_velocities_[i] = hw_commands_[i]; RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[0], + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i], info_.joints[i].name.c_str()); } diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index 603939229..0ecf1bfdc 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -23,6 +23,7 @@ ros2_control_demo_example_8 ros2_control_demo_example_9 ros2_control_demo_example_11 + ros2_control_demo_example_12 ament_cmake