diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.base_station.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.base_station.launch.py new file mode 100644 index 00000000..c7146a00 --- /dev/null +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.base_station.launch.py @@ -0,0 +1,55 @@ +# Copyright 2023 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 +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + # -- Declare arguments -- + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="arm_bringup", + description='Package with the controller\'s configuration in "config" folder.', + ) + ) + + # -- Initialize Arguments -- + runtime_config_package = LaunchConfiguration("runtime_config_package") + + # -- Building Path Files -- + joystick_config_file = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), 'config', 'joystick.yaml'] + ) + + # -- Node Definitions -- + joystick_publisher = Node( + package='teleop', + executable='joystick', + name='joystick', + output='screen', + parameters=[joystick_config_file], + ) + + return LaunchDescription( + declared_arguments + + [ + joystick_publisher, + ] + ) diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py new file mode 100644 index 00000000..ff6df579 --- /dev/null +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py @@ -0,0 +1,238 @@ +# Copyright 2023 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, LaunchContext +from launch.actions import RegisterEventHandler, DeclareLaunchArgument, TimerAction +from launch.event_handlers import OnProcessExit, OnProcessStart +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( + "runtime_config_package", + default_value="arm_bringup", + description='Package with the controller\'s configuration in "config" folder.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="athena_arm_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="description", + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="athena_arm.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="manual_arm_joint_by_joint_controller", + choices=["manual_arm_joint_by_joint_controller"], + description="Robot controller to start.", + ) + ) + + # -- Initialize Arguments -- + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + robot_controller = LaunchConfiguration("robot_controller") + + # -- Building Path Files -- + robot_description_path = PathJoinSubstitution( + [FindPackageShare("description"), "urdf", "athena_arm.urdf.xacro"] + ) + robot_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + + # -- Additional Configuration Setup -- + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + robot_description_path, + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # -- Node Definitions -- + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + robot_controller_names = [robot_controller] + robot_controller_spawners = [] + for controller in robot_controller_names: + robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + + inactive_robot_controller_names = ["manual_arm_cylindrical_controller", "joint_trajectory_controller", "arm_velocity_controller"] + inactive_robot_controller_spawners = [] + for controller in inactive_robot_controller_names: + inactive_robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager", "--inactive"], + ) + ] + + # Handle switching between controllers + controller_switcher_node = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=inactive_robot_controller_spawners[-1], + on_exit=[TimerAction( + period=3.0, + actions=[Node( + package="arm_bringup", + executable="controller_switcher.py", + name="controller_switcher", + output="screen" + )] + )], + ) + ) + + # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node + delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[ + TimerAction( + period=3.0, + actions=[joint_state_broadcaster_spawner], + ), + ], + ) + ) + + # Delay loading and activation of robot_controller_names after `joint_state_broadcaster` + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(robot_controller_spawners): + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + robot_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + + # Delay start of inactive_robot_controller_names after other controllers + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(inactive_robot_controller_spawners): + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + inactive_robot_controller_spawners[i - 1] + if i > 0 + else robot_controller_spawners[-1] + ), + on_exit=[controller], + ) + ) + ] + + return LaunchDescription( + declared_arguments + + [ + control_node, + robot_state_pub_node, + delay_joint_state_broadcaster_spawner_after_ros2_control_node, + controller_switcher_node, + ] + + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + ) diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.base_station.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.base_station.launch.py new file mode 100644 index 00000000..e976ab7b --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.base_station.launch.py @@ -0,0 +1,87 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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 +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # -- Declare arguments -- + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="drive_bringup", + description='Package with the controller\'s configuration in "config" folder.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "joystick_config", + default_value="joystick.yaml", + description="YAML file with the joystick configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "teleop_twist_config", + default_value="teleop_twist.yaml", + description="YAML file with the teleop_twist_node configuration.", + ) + ) + + # -- Initialize Arguments -- + runtime_config_package = LaunchConfiguration("runtime_config_package") + joystick_config = LaunchConfiguration("joystick_config") + teleop_twist_config = LaunchConfiguration("teleop_twist_config") + + # -- Building Path Files -- + joystick_config = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", joystick_config] + ) + teleop_twist_config = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", teleop_twist_config] + ) + + # -- Node Definitions -- + joystick_publisher = Node( + package='teleop', + executable='joystick', + name='joystick', + output='screen', + parameters=[joystick_config], + remappings=[ + ('controller_input', 'joy'), + ('/controller_input', '/joy'), + ], + ) + + teleop_twist_joy = Node( + package='teleop_twist_joy', + executable='teleop_node', + name='teleop_twist_joy', + output='screen', + parameters=[teleop_twist_config], + ) + + return LaunchDescription( + declared_arguments + + [ + joystick_publisher, + teleop_twist_joy, + ] + ) diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.jetson.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.jetson.launch.py new file mode 100644 index 00000000..afaa13fc --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.jetson.launch.py @@ -0,0 +1,267 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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, TimerAction +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 + + +def generate_launch_description(): + # -- Declare arguments -- + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="drive_bringup", + description='Package with the controller\'s configuration in "config" folder.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="athena_drive_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="description", + description="Description package with robot URDF/xacro files.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="athena_drive.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="single_ackermann_controller", + choices=["single_ackermann_controller", "ackermann_steering_controller"], + description="Robot controller to start.", + ) + ) + + # -- Initialize Arguments -- + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + robot_controller = LaunchConfiguration("robot_controller") + + # -- Building Path Files -- + robot_description_path = PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ) + robot_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + + # -- Additional Configuration Setup -- + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + robot_description_path, + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + # -- Node Definitions -- + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ("/single_ackermann_controller/reference", "/joy"), + ("/ackermann_steering_controller/reference", "/cmd_vel"), + ], + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_names = [robot_controller] + robot_controller_spawners = [] + for controller in robot_controller_names: + robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + + # GPIO controller spawners for LED and Killswitch + gpio_controller_names = ["led_gpio_controller", "killswitch_gpio_controller"] + gpio_controller_spawners = [] + for controller in gpio_controller_names: + gpio_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + + inactive_robot_controller_names = ["ackermann_steering_controller", "drive_velocity_controller", "drive_position_controller"] + inactive_robot_controller_spawners = [] + for controller in inactive_robot_controller_names: + inactive_robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager", "--inactive"], + ) + ] + + # Delay GPIO controller spawners after joint_state_broadcaster + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(gpio_controller_spawners): + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + gpio_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + + controller_switcher_node = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=inactive_robot_controller_spawners[-1], + on_exit=[TimerAction( + period=3.0, + actions=[Node( + package="drive_bringup", + executable="controller_switcher.py", + name="controller_switcher", + output="screen" + )] + )], + ) + ) + + delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[ + TimerAction( + period=5.0, + actions=[joint_state_broadcaster_spawner], + ), + ], + ) + ) + + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(robot_controller_spawners): + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + robot_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(inactive_robot_controller_spawners): + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + inactive_robot_controller_spawners[i - 1] + if i > 0 + else robot_controller_spawners[-1] + ), + on_exit=[controller], + ) + ) + ] + + return LaunchDescription( + declared_arguments + + [ + control_node, + robot_state_pub_node, + delay_joint_state_broadcaster_spawner_after_ros2_control_node, + controller_switcher_node, + ] + + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner + )