From 3139a9065d9bc00413192b27a49e2fb5d4426c7e Mon Sep 17 00:00:00 2001 From: huzaifa <84243533+huzzu7@users.noreply.github.com> Date: Fri, 28 Jun 2024 08:20:15 -0400 Subject: [PATCH] Ackermann steering example (#349) --- doc/index.rst | 2 + gz_ros2_control_demos/CMakeLists.txt | 7 + .../config/ackermann_drive_controller.yaml | 30 +++ .../examples/example_ackermann_drive.cpp | 54 +++++ .../launch/ackermann_drive_example.launch.py | 106 +++++++++ gz_ros2_control_demos/package.xml | 1 + .../urdf/test_ackermann_drive.xacro.urdf | 219 ++++++++++++++++++ 7 files changed, 419 insertions(+) create mode 100644 gz_ros2_control_demos/config/ackermann_drive_controller.yaml create mode 100644 gz_ros2_control_demos/examples/example_ackermann_drive.cpp create mode 100644 gz_ros2_control_demos/launch/ackermann_drive_example.launch.py create mode 100644 gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf diff --git a/doc/index.rst b/doc/index.rst index 7ff7ebcc..1e65767e 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -244,6 +244,7 @@ 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. @@ -251,6 +252,7 @@ When the Gazebo world is launched you can run some of the following commands to 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 ----------------------------------------------------------- diff --git a/gz_ros2_control_demos/CMakeLists.txt b/gz_ros2_control_demos/CMakeLists.txt index 536ca06e..b991ac2b 100644 --- a/gz_ros2_control_demos/CMakeLists.txt +++ b/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} diff --git a/gz_ros2_control_demos/config/ackermann_drive_controller.yaml b/gz_ros2_control_demos/config/ackermann_drive_controller.yaml new file mode 100644 index 00000000..e834c438 --- /dev/null +++ b/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 diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp new file mode 100644 index 00000000..7e3b4414 --- /dev/null +++ b/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 +#include + +#include + +#include + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = + std::make_shared("ackermann_drive_test_node"); + + auto publisher = node->create_publisher( + "/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; +} diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py new file mode 100644 index 00000000..cb2bbf99 --- /dev/null +++ b/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'), + ]) diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index 72b5f1c2..4c25fb63 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -43,6 +43,7 @@ velocity_controllers diff_drive_controller tricycle_controller + ackermann_steering_controller xacro ament_cmake_gtest diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf new file mode 100644 index 00000000..0066f698 --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + + + + + + + $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + + +