From 56cfe25731e5018b72beb72a0f5dafdd5d2fb154 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 1 Dec 2024 17:04:03 +0100 Subject: [PATCH] Added test for: chaining and multiple joints, all passed! --- .../bringup/config/chaining_joint_test.yaml | 43 +++++ .../bringup/config/multiple_joints_test.yaml | 84 +++++++++ .../bringup/config/single_joint_test.yaml | 5 +- .../launch/chaining_joint_test.launch.py | 174 ++++++++++++++++++ .../launch/multiple_joints_test.launch.py | 162 ++++++++++++++++ .../test/bringup/launch/rviz.launch.py | 2 +- .../test/cl_commands/chaining_joint_test.txt | 5 + .../test/cl_commands/multiple_joints_test.txt | 3 + .../test/cl_commands/single_joint_test.txt | 2 +- .../test/urdf/chaining_joint_test.urdf.xacro | 35 ++++ .../joint_hardware_macros.urdf.xacro | 0 .../urdf/{ => core}/single_joint.urdf.xacro | 0 .../test/urdf/multiple_joints_test.urdf.xacro | 45 +++++ .../test/urdf/single_joint_test.urdf.xacro | 4 +- 14 files changed, 559 insertions(+), 5 deletions(-) create mode 100644 joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml create mode 100644 joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml create mode 100644 joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py create mode 100644 joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py create mode 100644 joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt create mode 100644 joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt create mode 100644 joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro rename joint_controller_ros2_control/test/urdf/{ => core}/joint_hardware_macros.urdf.xacro (100%) rename joint_controller_ros2_control/test/urdf/{ => core}/single_joint.urdf.xacro (100%) create mode 100644 joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro diff --git a/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml b/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml new file mode 100644 index 0000000..221db32 --- /dev/null +++ b/joint_controller_ros2_control/test/bringup/config/chaining_joint_test.yaml @@ -0,0 +1,43 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_controller: + type: joint_controller/JointController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +forward_position_controller: + ros__parameters: + joints: + - joint_controller/body_1_joint + interface_name: position + +joint_controller: + ros__parameters: + joint_names: + - body_1_joint + + joint_params: + body_1_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + + reference_interfaces: + - "position" + + frequency: 500.0 + + pid_gains: + body_1_joint: + p: 1.0 + d: 0.5 + i: 0.0 + ilimit: 1.0 diff --git a/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml b/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml new file mode 100644 index 0000000..5a26931 --- /dev/null +++ b/joint_controller_ros2_control/test/bringup/config/multiple_joints_test.yaml @@ -0,0 +1,84 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_controller: + type: joint_controller/JointController + + +joint_controller: + ros__parameters: + joint_names: + - body_1_joint + - body_2_joint + - body_3_joint + - body_4_joint + - body_5_joint + + joint_params: + body_1_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + body_2_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + body_3_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + body_4_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + body_5_joint: + position_max: 3.14 + position_min: -3.14 + position_offset: 0.0 + velocity_max: 10.0 + effort_max: 10.0 + + reference_interfaces: + - "position" + + frequency: 500.0 + + pid_gains: + body_1_joint: + p: 1.0 + d: 0.5 + i: 0.0 + ilimit: 1.0 + body_2_joint: + p: 10.0 + d: 0.5 + i: 0.0 + ilimit: 1.0 + body_3_joint: + p: 1.0 + d: 1.5 + i: 0.0 + ilimit: 1.0 + body_4_joint: + p: 10.0 + d: 5.0 + i: 0.0 + ilimit: 1.0 + body_5_joint: + p: 1.0 + d: 0.5 + i: 0.25 + ilimit: 1.0 + \ No newline at end of file diff --git a/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml index 603e82e..25aaca1 100644 --- a/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml +++ b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml @@ -21,7 +21,10 @@ joint_controller: position_offset: 0.0 velocity_max: 10.0 effort_max: 10.0 - + + reference_interfaces: + - "position" + frequency: 500.0 pid_gains: diff --git a/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py b/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py new file mode 100644 index 0000000..e5953c7 --- /dev/null +++ b/joint_controller_ros2_control/test/bringup/launch/chaining_joint_test.launch.py @@ -0,0 +1,174 @@ +# 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="joint_controller", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="chaining_joint_test.urdf.xacro", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type_file", + default_value="chaining_joint_test.yaml", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type", + default_value="joint_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), + "urdf", + 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' + ) + + load_forward_position_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'forward_position_controller'], + 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'])]) + gazebo_sim_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']), + launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items() + ) + + nodes = [ + 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], + ) + ), + + RegisterEventHandler( + event_handler=OnProcessExit( + target_action= load_joint_effort_controller, + on_exit=[load_forward_position_controller], + ) + ), + + robot_state_pub_node, + spawn_entity, + ] + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py b/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py new file mode 100644 index 0000000..3a1a82c --- /dev/null +++ b/joint_controller_ros2_control/test/bringup/launch/multiple_joints_test.launch.py @@ -0,0 +1,162 @@ +# 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="joint_controller", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="multiple_joints_test.urdf.xacro", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type_file", + default_value="multiple_joints_test.yaml", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "controller_type", + default_value="joint_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), + "urdf", + 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'])]) + gazebo_sim_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']), + launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items() + ) + + nodes = [ + 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/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py index d830fde..d04de51 100644 --- a/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py +++ b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "urdf_file", - default_value="single_joint_test.urdf.xacro", + default_value="multiple_joints_test.urdf.xacro", ) ) diff --git a/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt b/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt new file mode 100644 index 0000000..9ed9531 --- /dev/null +++ b/joint_controller_ros2_control/test/cl_commands/chaining_joint_test.txt @@ -0,0 +1,5 @@ +// Sending position 1.0 for joint "body_1_joint" through forward_position_controller + + +ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: +- 0.5" \ No newline at end of file diff --git a/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt b/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt new file mode 100644 index 0000000..2fdfc50 --- /dev/null +++ b/joint_controller_ros2_control/test/cl_commands/multiple_joints_test.txt @@ -0,0 +1,3 @@ +// Sending positions 1.0 for all joints "body_x_joint" + +ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: ['body_1_joint', 'body_2_joint', 'body_3_joint', 'body_4_joint', 'body_5_joint'], desired_position:[1.0, 1.0, 1.0, 1.0, 1.0], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0]}" diff --git a/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt b/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt index 0da172a..55df33e 100644 --- a/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt +++ b/joint_controller_ros2_control/test/cl_commands/single_joint_test.txt @@ -1,4 +1,4 @@ -// Wysyłanie pozycji 1.0 dla jointa "body_1_joint" +// Sending position 1.0 for joint "body_1_joint" ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'body_1_joint'}, desired_position:{1.0}, desired_velocity: {0.0}, kp_scale: {0.0}, kd_scale: {0.0}, feedforward_effort: {0.0}}" diff --git a/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro new file mode 100644 index 0000000..de61eec --- /dev/null +++ b/joint_controller_ros2_control/test/urdf/chaining_joint_test.urdf.xacro @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + $(find joint_controller)/config/chaining_joint_test.yaml + + + + \ No newline at end of file diff --git a/joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro b/joint_controller_ros2_control/test/urdf/core/joint_hardware_macros.urdf.xacro similarity index 100% rename from joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro rename to joint_controller_ros2_control/test/urdf/core/joint_hardware_macros.urdf.xacro diff --git a/joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro b/joint_controller_ros2_control/test/urdf/core/single_joint.urdf.xacro similarity index 100% rename from joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro rename to joint_controller_ros2_control/test/urdf/core/single_joint.urdf.xacro diff --git a/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro new file mode 100644 index 0000000..99c3f86 --- /dev/null +++ b/joint_controller_ros2_control/test/urdf/multiple_joints_test.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + $(find joint_controller)/config/multiple_joints_test.yaml + + + + + + + \ No newline at end of file diff --git a/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro index f805af9..4407149 100644 --- a/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro +++ b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro @@ -12,7 +12,7 @@ - + @@ -22,7 +22,7 @@ ign_ros2_control/IgnitionSystem - +