[Example 11] Carlike Robot using Bicycle Steering Controller (#316)

Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

.github/workflows/ci-ros-lint.yml

@@ -30,6 +30,7 @@ jobs:
           ros2_control_demo_example_8
           ros2_control_demo_example_9
           ros2_control_demo_example_10
+          ros2_control_demo_example_11
           ros2_control_demo_example_12
           ros2_control_demo_example_14
 

README.md

@@ -60,7 +60,7 @@ The following examples are part of this demo repository:
 
    *RRBot* with GPIO interfaces.
 
-* Example 11: "Car-like robot using steering controller library (tba.)"
+* Example 11: ["Car-like robot using steering controller library"](example_11)
 
 * Example 12: ["Controller chaining"](example_12)
 

doc/index.rst

@@ -70,7 +70,8 @@ Example 9: "Gazebo Classic"
 Example 10: "GPIO interfaces"
    Industrial robot with GPIO interfaces
 
-Example 11: "Car-like robot using steering controller library (tba.)"
+Example 11: "CarlikeBot"
+    *CarlikeBot* with a bicycle steering controller
 
 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*.
@@ -270,5 +271,6 @@ Examples
    Example 8: Using transmissions <../example_8/doc/userdoc.rst>
    Example 9: Gazebo classic <../example_9/doc/userdoc.rst>
    Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst>
+   Example 11: CarlikeBot <../example_11/doc/userdoc.rst>
    Example 12: Controller chaining <../example_12/doc/userdoc.rst>
    Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst>

example_11/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_11 LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+add_library(
+  ros2_control_demo_example_11
+  SHARED
+  hardware/carlikebot_system.cpp
+)
+target_compile_features(ros2_control_demo_example_11 PUBLIC cxx_std_17)
+target_include_directories(ros2_control_demo_example_11 PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+$<INSTALL_INTERFACE:include/ros2_control_demo_example_11>
+)
+ament_target_dependencies(
+  ros2_control_demo_example_11 PUBLIC
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_11_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_11.xml)
+
+# INSTALL
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include/ros2_control_demo_example_11
+)
+install(
+  DIRECTORY description/launch description/ros2_control description/urdf
+  DESTINATION share/ros2_control_demo_example_11
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_11
+)
+install(TARGETS ros2_control_demo_example_11
+  EXPORT export_ros2_control_demo_example_11
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  ament_add_pytest_test(example_11_urdf_xacro test/test_urdf_xacro.py)
+  ament_add_pytest_test(view_example_11_launch test/test_view_robot_launch.py)
+endif()
+
+## EXPORTS
+ament_export_targets(export_ros2_control_demo_example_11 HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

example_11/README.md

@@ -0,0 +1,6 @@
+# ros2_control_demo_example_11
+
+   *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with bicycle drive.
+   The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. However, since each pair of wheels (steering and traction) are controlled by one interface, a bicycle steering model is used.
+
+Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_11/doc/userdoc.html).

example_11/bringup/config/carlikebot_controllers.yaml

@@ -0,0 +1,29 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    bicycle_steering_controller:
+      type: bicycle_steering_controller/BicycleSteeringController
+
+
+bicycle_steering_controller:
+  ros__parameters:
+    wheelbase: 0.325
+    front_wheel_radius: 0.05
+    rear_wheel_radius: 0.05
+    front_steering: true
+    reference_timeout: 2.0
+    rear_wheels_names: ['virtual_rear_wheel_joint']
+    front_wheels_names: ['virtual_front_wheel_joint']
+    open_loop: false
+    velocity_rolling_window_size: 10
+    base_frame_id: base_link
+    odom_frame_id: odom
+    enable_odom_tf: true
+    twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    position_feedback: false
+    use_stamped_vel: true

example_11/bringup/launch/carlikebot.launch.py

@@ -0,0 +1,151 @@
+# Copyright 2020 ros2_control Development Team
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "remap_odometry_tf",
+            default_value="false",
+            description="Remap odometry TF from the steering controller to the TF tree.",
+        )
+    )
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+    remap_odometry_tf = LaunchConfiguration("remap_odometry_tf")
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_example_11"),
+            "config",
+            "carlikebot_controllers.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_description"),
+            "carlikebot/rviz",
+            "carlikebot.rviz",
+        ]
+    )
+
+    # the steering controller libraries by default publish odometry on a separate topic than /tf
+    control_node_remapped = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+            ("/bicycle_steering_controller/tf_odometry", "/tf"),
+        ],
+        condition=IfCondition(remap_odometry_tf),
+    )
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
+        condition=UnlessCondition(remap_odometry_tf),
+    )
+    robot_state_pub_bicycle_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    robot_bicycle_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    # Delay start of robot_controller after `joint_state_broadcaster`
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_bicycle_controller_spawner],
+        )
+    )
+
+    nodes = [
+        control_node,
+        control_node_remapped,
+        robot_state_pub_bicycle_node,
+        joint_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)