Ackermann steering example (#349)

doc/index.rst

@@ -244,13 +244,15 @@ You can run some of the mobile robots running the following commands:
 
   ros2 launch gz_ros2_control_demos diff_drive_example.launch.py
   ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py
+  ros2 launch gz_ros2_control_demos ackermann_drive_example.launch.py
 
 When the Gazebo world is launched you can run some of the following commands to move the robots.
 
 .. code-block:: shell
 
   ros2 run gz_ros2_control_demos example_diff_drive
   ros2 run gz_ros2_control_demos example_tricycle_drive
+  ros2 run gz_ros2_control_demos example_ackermann_drive
 
 Gripper
 -----------------------------------------------------------

gz_ros2_control_demos/CMakeLists.txt

@@ -60,6 +60,12 @@ ament_target_dependencies(example_tricycle_drive
   geometry_msgs
 )
 
+add_executable(example_ackermann_drive examples/example_ackermann_drive.cpp)
+ament_target_dependencies(example_ackermann_drive
+  rclcpp
+  geometry_msgs
+)
+
 add_executable(example_gripper examples/example_gripper.cpp)
 ament_target_dependencies(example_gripper
   rclcpp
@@ -81,6 +87,7 @@ install(
     example_effort
     example_diff_drive
     example_tricycle_drive
+    example_ackermann_drive
     example_gripper
   DESTINATION
     lib/${PROJECT_NAME}

gz_ros2_control_demos/config/ackermann_drive_controller.yaml

@@ -0,0 +1,30 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+    ackermann_steering_controller:
+      type: 'ackermann_steering_controller/AckermannSteeringController'
+
+ackermann_steering_controller:
+  ros__parameters:
+    wheelbase: 1.7
+    front_wheel_track: 1.0
+    rear_wheel_track: 1.0
+    front_wheels_radius: 0.3
+    rear_wheels_radius: 0.3
+    front_steering: true
+    reference_timeout: 2.0
+    rear_wheels_names: ['rear_left_wheel_joint', 'rear_right_wheel_joint']
+    front_wheels_names: ['left_wheel_steering_joint', 'right_wheel_steering_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

gz_ros2_control_demos/examples/example_ackermann_drive.cpp

@@ -0,0 +1,54 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <chrono>
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  std::shared_ptr<rclcpp::Node> node =
+    std::make_shared<rclcpp::Node>("ackermann_drive_test_node");
+
+  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
+    "/ackermann_steering_controller/reference_unstamped", 10);
+
+  RCLCPP_INFO(node->get_logger(), "node created");
+
+  geometry_msgs::msg::Twist command;
+
+  command.linear.x = 0.5;
+  command.linear.y = 0.0;
+  command.linear.z = 0.0;
+
+  command.angular.x = 0.0;
+  command.angular.y = 0.0;
+  command.angular.z = 0.3;
+
+  while (1) {
+    publisher->publish(command);
+    std::this_thread::sleep_for(50ms);
+    rclcpp::spin_some(node);
+  }
+  rclcpp::shutdown();
+
+  return 0;
+}

gz_ros2_control_demos/launch/ackermann_drive_example.launch.py

@@ -0,0 +1,106 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Launch Arguments
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name='xacro')]),
+            ' ',
+            PathJoinSubstitution(
+                [FindPackageShare('gz_ros2_control_demos'),
+                 'urdf', 'test_ackermann_drive.xacro.urdf']
+            ),
+        ]
+    )
+    robot_description = {'robot_description': robot_description_content}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description]
+    )
+
+    gz_spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        arguments=['-topic', 'robot_description', '-name',
+                   'ackermann', '-allow_renaming', 'true'],
+    )
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_ackermann_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'ackermann_steering_controller'],
+        output='screen'
+    )
+
+    # Bridge
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        bridge,
+        # Launch gazebo environment
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
+                                       'launch',
+                                       'gz_sim.launch.py'])]),
+            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=gz_spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_ackermann_controller],
+            )
+        ),
+        node_robot_state_publisher,
+        gz_spawn_entity,
+        # Launch Arguments
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value=use_sim_time,
+            description='If true, use simulated clock'),
+    ])

gz_ros2_control_demos/package.xml

@@ -43,6 +43,7 @@
   <exec_depend>velocity_controllers</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
   <exec_depend>tricycle_controller</exec_depend>
+  <exec_depend>ackermann_steering_controller</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>