From f9e3e92beda90746742672dc475ea1595bbbd481 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 14 Sep 2024 17:25:38 +0200 Subject: [PATCH] Succesfully runned simulation with ros2 control plugin and controllers --- .../description/meldog_gazebo.urdf.xacro | 2 + .../ros2_control_gazebo.urdf.xacro | 28 ++-- .../ros2_control_joint_macros.urdf.xacro | 13 +- .../transmission_macros.urdf.xacro | 14 +- .../meldog_gazebo_sim_ros2_control.launch.py | 157 ++++++++++++++++++ .../joint_trajectory_controller_test.yaml | 78 +++------ 6 files changed, 213 insertions(+), 79 deletions(-) create mode 100644 src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py diff --git a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro index d5c8ea0..af40456 100644 --- a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro +++ b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro @@ -55,4 +55,6 @@ + + \ No newline at end of file diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro index 509198f..b142bbf 100644 --- a/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro +++ b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro @@ -7,29 +7,29 @@ ign_ros2_control/IgnitionSystem - + - + - - - - + + + + - - - - + + + + - - - - + + + + diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro index 8263dbd..734c090 100644 --- a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro +++ b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro @@ -1,9 +1,9 @@ - + - + ${lower} @@ -13,11 +13,10 @@ -${actuator_max_velocity} ${actuator_max_velocity} - - - -${actuator_max_torque} - ${actuator_max_torque} - + + -${actuator_max_torque} + ${actuator_max_torque} + diff --git a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro index 6e9a210..8ffc223 100644 --- a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro +++ b/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro @@ -2,7 +2,7 @@ - + transmission_interface/SimpleTransmission @@ -14,8 +14,8 @@ - - + + transmission_interface/FourBarLinkageTransmission ${reduction} @@ -43,9 +43,9 @@ - - - - + + + + \ No newline at end of file diff --git a/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py new file mode 100644 index 0000000..070949c --- /dev/null +++ b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py @@ -0,0 +1,157 @@ +# Copyright 2020 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. + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch_ros.parameter_descriptions import ParameterValue + +from launch_ros.actions import Node + + +def generate_launch_description(): + + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "package", + default_value="meldog_simple_description", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="meldog_gazebo.urdf.xacro", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type_file", + default_value="joint_trajectory_controller_test.yaml", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type", + default_value="joint_trajectory_controller", + ) + ) + + # Initialize Arguments + package_name = LaunchConfiguration("package") + urdf_file = LaunchConfiguration("urdf_file") + controller_file = LaunchConfiguration("controller_type_file") + controller_type = LaunchConfiguration("controller_type") + + + # Get URDF via xacro + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(package_name), + "description", + urdf_file, + ] + ), + ] + ), value_type=str) + + # Set up dictionary parameters: + robot_description = {"robot_description": robot_description_content} + use_sim_time = {"use_sim_time": True} + + # Load world for gazebo sim + world = PathJoinSubstitution( + [ + FindPackageShare(package_name), + "worlds", + 'empty.world' + ] + ) + + # robot_state_publisher node: + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description, use_sim_time], + ) + + # Spawn Meldog: + spawn_entity = Node(package='ros_gz_sim', executable='create', + arguments=['-topic', 'robot_description', + '-name', 'Meldog'], + output='screen') + + # Load joint_state_broadcaster + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + # Load joint_trajectory_controller + load_joint_effort_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type], + output='screen' + ) + + # Bridge between ros2 and Ignition Gazebo + ros2_gazebo_sim_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]) + + nodes = [ + ros2_gazebo_sim_bridge, + + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + + RegisterEventHandler( + event_handler=OnProcessExit( + target_action= load_joint_state_broadcaster, + on_exit=[load_joint_effort_controller], + ) + ), + + robot_state_pub_node, + spawn_entity, + ] + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml index ec3c614..48b7591 100644 --- a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml +++ b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 200.0 # Hz + update_rate: 1000 # Hz use_sim_time: true joint_trajectory_controller: @@ -52,92 +52,68 @@ joint_trajectory_controller: state_interfaces: - position - velocity - action_monitor_rate: 200.0 - state_publish_rate: 200.0 # Hz + action_monitor_rate: 1000.0 + state_publish_rate: 1000.0 # Hz allow_nonzero_velocity_at_trajectory_end: false gains: FLH_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 FRH_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BLH_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BRH_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 FLT_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 FRT_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BLT_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BRT_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 FLS_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 FRS_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BLS_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 + p: 350.0 BRS_joint: - angle_wraparound: false - d: 0.1 - ff_velocity_scale: 0.0 + d: 2.5 i: 0.0 i_clamp: 0.0 - p: 5.0 \ No newline at end of file + p: 350.0 \ No newline at end of file