diff --git a/src/bringup/package.xml b/src/bringup/package.xml index d69446e..d681fa1 100644 --- a/src/bringup/package.xml +++ b/src/bringup/package.xml @@ -14,10 +14,6 @@ rclpy controller_manager_msgs - arm_bringup - drive_bringup - science_bringup - ament_cmake diff --git a/src/description/config/athena_drive_sim_controllers.yaml b/src/description/config/athena_drive_sim_controllers.yaml deleted file mode 100644 index 52e9853..0000000 --- a/src/description/config/athena_drive_sim_controllers.yaml +++ /dev/null @@ -1,35 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - ackermann_steering_controller: - type: ackermann_steering_controller/AckermannSteeringController - - rear_steer_position_controller: - type: position_controllers/JointGroupPositionController - - -ackermann_steering_controller: - ros__parameters: - - rear_wheels_names: ["propulsion_fl_joint", "propulsion_fr_joint"] - - front_wheels_names: ["steer_fl_joint", "steer_fr_joint"] - - front_wheel_track: 0.6604 - rear_wheel_track: 0.6604 - wheelbase: 0.8382 - front_wheels_radius: 0.254 - rear_wheels_radius: 0.254 - use_stamped_vel: true - - -rear_steer_position_controller: - ros__parameters: - joints: - - steer_br_joint - - steer_bl_joint - interface_name: position diff --git a/src/description/launch/display.launch.py b/src/description/launch/display.launch.py index 4d50279..39ef2fc 100644 --- a/src/description/launch/display.launch.py +++ b/src/description/launch/display.launch.py @@ -24,7 +24,6 @@ def generate_launch_description(): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", robot_description_path, - " use_mock_hardware:=true", ] ) diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/athena_arm.urdf.xacro index ddaa25b..a8e42fc 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/athena_arm.urdf.xacro @@ -3,10 +3,7 @@ - - - - + @@ -40,10 +37,10 @@ - + - + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index dbce67b..97323b7 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -4,9 +4,8 @@ - - - + + @@ -37,16 +36,14 @@ - + - + diff --git a/src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro b/src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro index 0c71739..54b9f8d 100644 --- a/src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro +++ b/src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro @@ -1,141 +1,112 @@ - - - - - - gz_ros2_control/GazeboSimSystem - - - - mock_components/GenericSystem - ${mock_sensor_commands} - - - - mock_components/GenericSystem - - - - - - -1.0471975512 - 1.0471975512 - - - 0.0 - - - - - - - -1.0471975512 - 1.0471975512 - - - 0.0 - - - - - - - 0 - 6.2832 - - - 0.0 - - - - - - - 0 - 6.2832 - - - 0.0 - - - - - - - 0 - 6.28318530718 - - - 0.0 - - - - - - - 0 - 6.2832 - - - 0.0 - - - - - - - - - 0.0 - - - - - - - - 0.0 - - - - - - - - 0.0 - - - - - - - - 0.0 - - - - - - - - - - ${simulation_controllers} - - ${prefix}controller_manager - - - - - + prefix use_sim:=false + simulation_controllers"> + + + + + gz_ros2_control/GazeboSimSystem + + + + mock_components/GenericSystem + + + + + + 0 + 6.2832 + + + 0.0 + + + + + + + 0 + 6.2832 + + + 0.0 + + + + + + + 0 + 6.28318530718 + + + 0.0 + + + + + + + 0 + 6.2832 + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + + + ${simulation_controllers} + + ${prefix}controller_manager + + + + + diff --git a/src/description/urdf/athena_drive/athena_drive_macro.xacro b/src/description/urdf/athena_drive/athena_drive_macro.xacro index e64baee..f5707ea 100644 --- a/src/description/urdf/athena_drive/athena_drive_macro.xacro +++ b/src/description/urdf/athena_drive/athena_drive_macro.xacro @@ -177,7 +177,7 @@ - + @@ -217,7 +217,7 @@ - + @@ -226,7 +226,7 @@ - + diff --git a/src/simulation/CMakeLists.txt b/src/simulation/CMakeLists.txt index 5498281..7b28113 100644 --- a/src/simulation/CMakeLists.txt +++ b/src/simulation/CMakeLists.txt @@ -6,12 +6,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(xacro REQUIRED) -find_package(robot_state_publisher REQUIRED) -find_package(ros2_control REQUIRED) -find_package(ros2_controllers REQUIRED) find_package(ros_gz_sim REQUIRED) find_package(ros_gz_bridge REQUIRED) @@ -26,12 +20,6 @@ foreach(dir urdf meshes config launch worlds textures models) endforeach() ament_export_dependencies( - rclcpp - std_msgs - xacro - robot_state_publisher - ros2_control - ros2_controllers ros_gz_sim ros_gz_bridge ) diff --git a/src/simulation/launch/bringup.launch.py b/src/simulation/launch/bringup.launch.py deleted file mode 100644 index b6fd24e..0000000 --- a/src/simulation/launch/bringup.launch.py +++ /dev/null @@ -1,80 +0,0 @@ -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - -ARGUMENTS = [ - DeclareLaunchArgument( - 'namespace', - default_value='', - description='Robot namespace' - ), - DeclareLaunchArgument( - 'rviz', - default_value='false', - choices=['true', 'false'], - description='Start RViz for visualization' - ), - DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - choices=['true', 'false'], - description='Use simulation time from Gazebo' - ), - DeclareLaunchArgument( - 'world', - default_value='empty.sdf', - description='Gazebo world file to load' - ), - DeclareLaunchArgument( - 'world_name', - default_value='default', - description='Name of the world inside Gazebo' - ), -] - -def generate_launch_description(): - pkg_sim = get_package_share_directory('simulation') - - gazebo_launch = PathJoinSubstitution( - [pkg_sim, 'launch', 'gz_sim.launch.py']) - robot_spawn_launch = PathJoinSubstitution( - [pkg_sim, 'launch', 'spawn.launch.py']) - bridge_launch = PathJoinSubstitution( - [pkg_sim, 'launch', 'bridge.launch.py']) - control_launch = PathJoinSubstitution( - [pkg_sim, 'launch', 'control.launch.py']) - - gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([gazebo_launch]), - launch_arguments=[ - ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('world', LaunchConfiguration('world')) - ] - ) - - robot_spawn = IncludeLaunchDescription( - PythonLaunchDescriptionSource([robot_spawn_launch]), - launch_arguments=[ - ('namespace', LaunchConfiguration('namespace')), - ('rviz', LaunchConfiguration('rviz')), - ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('world_name', LaunchConfiguration('world_name')) - ] - ) - - bridge = IncludeLaunchDescription( - PythonLaunchDescriptionSource([bridge_launch]) - ) - - control = IncludeLaunchDescription( - PythonLaunchDescriptionSource([control_launch]) - ) - - ld = LaunchDescription(ARGUMENTS) - ld.add_action(gazebo) - ld.add_action(robot_spawn) - ld.add_action(bridge) - ld.add_action(control) - return ld \ No newline at end of file diff --git a/src/simulation/launch/control.launch.py b/src/simulation/launch/control.launch.py deleted file mode 100644 index 2d6bb94..0000000 --- a/src/simulation/launch/control.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch_ros.actions import Node - -def generate_launch_description(): - joint_state_broadcaster_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=['joint_state_broadcaster'], - output='screen' - ) - - ackermann_controller_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=['ackermann_steering_controller'], - output='screen' - ) - - delayed_ackermann_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[ackermann_controller_spawner], - ) - ) - - ld = LaunchDescription() - ld.add_action(joint_state_broadcaster_spawner) - ld.add_action(delayed_ackermann_controller_spawner) - return ld \ No newline at end of file diff --git a/src/simulation/launch/sim_bringup.launch.py b/src/simulation/launch/sim_bringup.launch.py new file mode 100644 index 0000000..6e89d79 --- /dev/null +++ b/src/simulation/launch/sim_bringup.launch.py @@ -0,0 +1,42 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +ARGUMENTS = [ + DeclareLaunchArgument( + 'world', + default_value='empty.sdf', + description='Gazebo world file to load' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + choices=['true', 'false'], + description='Use simulation time from Gazebo' + ), +] + + +def generate_launch_description(): + pkg_sim = get_package_share_directory('simulation') + + gazebo_launch = PathJoinSubstitution([pkg_sim, 'launch', 'gz_sim.launch.py']) + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('use_sim_time', LaunchConfiguration('use_sim_time')), + ('world', LaunchConfiguration('world')) + ] + ) + + bridge_launch = PathJoinSubstitution([pkg_sim, 'launch', 'bridge.launch.py']) + bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([bridge_launch]) + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(gazebo) + ld.add_action(bridge) + return ld diff --git a/src/simulation/launch/spawn.launch.py b/src/simulation/launch/spawn.launch.py deleted file mode 100644 index 4b5bdf0..0000000 --- a/src/simulation/launch/spawn.launch.py +++ /dev/null @@ -1,79 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch import conditions -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.substitutions import LaunchConfiguration, Command -from launch_ros.actions import Node, PushRosNamespace -from launch_ros.parameter_descriptions import ParameterValue - -ARGUMENTS = [ - DeclareLaunchArgument('rviz', default_value='false', - choices=['true', 'false'], - description='Start rviz.'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), - DeclareLaunchArgument('namespace', default_value='', - description='Robot namespace'), - -] - -def generate_launch_description(): - pkg_share = get_package_share_directory('description') - - urdf_file = os.path.join(pkg_share, 'urdf', 'athena_drive.urdf.xacro') - controllers_file = os.path.join(pkg_share, 'config', 'athena_drive_sim_controllers.yaml') - - - namespace = LaunchConfiguration('namespace') - robot_name = 'rover' - - robot_description_content = Command([ - 'xacro ', urdf_file, - ' use_mock_hardware:=true', - ' sim_gazebo:=true', - f' simulation_controllers:={controllers_file}' - ]) - - spawn_robot_group_action = GroupAction([ - PushRosNamespace(namespace), - - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'robot_description': ParameterValue( - robot_description_content, - value_type=str - ), - 'use_sim_time': LaunchConfiguration('use_sim_time') - }] - ), - - Node( - package='ros_gz_sim', - executable='create', - arguments=['-name', robot_name, - '-x', '0.0', - '-y', '0.0', - '-z', '3.0', - '-Y', '0.0', - '-topic', 'robot_description'], - output='screen' - ), - - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - condition=conditions.IfCondition(LaunchConfiguration('rviz')) - ), - ]) - - ld = LaunchDescription(ARGUMENTS) - ld.add_action(spawn_robot_group_action) - return ld \ No newline at end of file diff --git a/src/simulation/package.xml b/src/simulation/package.xml index a1df672..addc71c 100644 --- a/src/simulation/package.xml +++ b/src/simulation/package.xml @@ -11,16 +11,6 @@ ros_gz_sim ros_gz_bridge - gz_ros2_control - - xacro - ros2_control - ros2_controllers - controller_manager - ackermann_steering_controller - description - - robot_state_publisher ament_lint_auto ament_lint_common diff --git a/src/subsystems/drive/drive_bringup/CMakeLists.txt b/src/subsystems/drive/drive_bringup/CMakeLists.txt index 58b628d..d81d5f2 100644 --- a/src/subsystems/drive/drive_bringup/CMakeLists.txt +++ b/src/subsystems/drive/drive_bringup/CMakeLists.txt @@ -16,12 +16,7 @@ install( DESTINATION share/${PROJECT_NAME} ) -install(PROGRAMS - scripts/controller_switcher.py - DESTINATION lib/${PROJECT_NAME} -) - if(BUILD_TESTING) endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/LICENSE b/src/subsystems/drive/drive_bringup/LICENSE deleted file mode 100644 index d645695..0000000 --- a/src/subsystems/drive/drive_bringup/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index 93a84ff..dc5e719 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -1,24 +1,3 @@ -# Copyright (c) 20224 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - controller_manager: ros__parameters: update_rate: 100 # Hz @@ -46,7 +25,7 @@ ackermann_steering_controller: # unsure why but ackermann controller only goes about half of scale_linear.x in teleop_twist # ackermann controller turns to at max scale_angular.yaw*pi. Seems to ignore how much you input, relies more on how fast rover is going ros__parameters: - rear_wheels_names: ["propulsion_bl_joint", "propulsion_br_joint"] # This is traction control "propulsion_bl_joint", "propulsion_br_joint", + rear_wheels_names: ["propulsion_bl_joint", "propulsion_br_joint"] front_wheels_names: ["steer_fl_joint", "steer_fr_joint"] # This is steering, don't ask why its just called front wheels # front_steering_names: ["steer_fl_joint", "steer_fr_joint"] front_wheel_track: 0.6604 @@ -90,4 +69,4 @@ drive_position_controller: - propulsion_br_joint - steer_fl_joint - steer_fr_joint - interface_name: position + interface_name: position \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml b/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml index 9d83c23..82872e5 100644 --- a/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml +++ b/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml @@ -49,9 +49,9 @@ publisher_joint_trajectory_controller: positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] joints: - - suspension_left_joint + - suspension_front_left_joint - steer_fl_joint - propulsion_fl_joint - swerve_module_back_joint - propulsion_bl_joint - - suspension_right_joint + - suspension_front_right_joint diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 2a52b8f..ee81e09 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -1,367 +1,199 @@ -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, +) 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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # -- Declare arguments -- - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "use_sim", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="drive_bringup", - description='Package with the controller\'s configuration in "config" folder. \ - Usually the argument is not set, it enables use of a custom setup.', - ) - ) - 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.", - ) - ) - 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. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="athena_drive.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "rviz_file", - default_value="athena_drive.rviz", - description="Rviz config file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - 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 used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="single_ackermann_controller", - choices=["single_ackermann_controller", "ackermann_steering_controller"], - description="Robot controller to start.", - ) + use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation mode (true) or real hardware mode (false)", ) - # -- Initialize Arguments -- - use_sim = LaunchConfiguration("use_sim") - runtime_config_package = LaunchConfiguration("runtime_config_package") - joystick_config = LaunchConfiguration("joystick_config") - teleop_twist_config = LaunchConfiguration("teleop_twist_config") - controllers_file = LaunchConfiguration("controllers_file") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - rviz_file = LaunchConfiguration("rviz_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] - ) - joystick_config = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", joystick_config] - ) - teleop_twist_config = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", teleop_twist_config] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", rviz_file] + runtime_config_package_arg = DeclareLaunchArgument( + "runtime_config_package", + default_value="drive_bringup", + description="Package with the controller configuration in 'config' folder", ) - # -- 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, - " ", - ] + description_package_arg = DeclareLaunchArgument( + "description_package", + default_value="description", + description="Description package with robot URDF/xacro files", ) - 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"), - ], + description_file_arg = DeclareLaunchArgument( + "description_file", + default_value="athena_drive.urdf.xacro", + description="URDF/XACRO description file with the robot", ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], + rviz_file_arg = DeclareLaunchArgument( + "rviz_file", + default_value="athena_drive.rviz", + description="RViz config file", ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(use_sim), + prefix_arg = DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names for multi-robot setup", ) - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + robot_controller_arg = DeclareLaunchArgument( + "robot_controller", + default_value="ackermann_steering_controller", + choices=["single_ackermann_controller", "ackermann_steering_controller"], + description="Robot controller to start", ) - joint_state_publisher_gui_node = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui' - ) - - joint_state_publisher = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - output='screen' + start_rviz_arg = DeclareLaunchArgument( + "start_rviz", + default_value="false", + choices=["true", "false"], + description="Start RViz2 for visualization", ) - 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 = ["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"], - ) - ] - - 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" - )] - )], - ) + world_arg = DeclareLaunchArgument( + "world", + default_value="empty.sdf", + description="Gazebo world file to load (only used when use_sim:=true)", ) - delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=control_node, - on_start=[ - TimerAction( - period=5.0, # Increased delay to ensure hardware interfaces are fully initialized - actions=[joint_state_broadcaster_spawner], - ), - ], - ) - ) - - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - 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], - ) - ) - ] - - umdloop_can_node = Node( - package='umdloop_can', - executable='can_node', - name='can_node', - output='log', - arguments=['--ros-args', '--log-level', 'fatal'] - ) - - delay_can_node_after_control_node = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=control_node, - on_start=[ - TimerAction( - period=1.0, # Small delay to let control node initialize - actions=[umdloop_can_node], - ), - ], - ) - ) - - 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], + use_sim = LaunchConfiguration("use_sim") + runtime_config_package = LaunchConfiguration("runtime_config_package") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + robot_controller = LaunchConfiguration("robot_controller") + start_rviz = LaunchConfiguration("start_rviz") + rviz_file = LaunchConfiguration("rviz_file") + world = LaunchConfiguration("world") + + robot_controllers_path = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", "athena_drive_controllers.yaml"] + ) + + sim_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare("simulation"), + "launch", + "sim_bringup.launch.py" + ]) + ]), + launch_arguments={ + "world": world, + "use_sim_time": "true", + }.items(), + condition=IfCondition(use_sim), ) - - return LaunchDescription( - declared_arguments + - [ - control_node, - robot_state_pub_node, - joystick_publisher, - teleop_twist_joy, - joint_state_publisher, - # delay_can_node_after_control_node, - delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_rviz_after_joint_state_broadcaster_spawner, - controller_switcher_node, - ] - + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner - + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner - ) \ No newline at end of file + robot_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "launch", + "robot_description.launch.py" + ]) + ]), + launch_arguments={ + "description_package": description_package, + "description_file": description_file, + "prefix": prefix, + "use_sim": use_sim, + "simulation_controllers": robot_controllers_path, + }.items(), + ) + + teleop = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "launch", + "teleop.launch.py" + ]) + ]), + launch_arguments={ + "runtime_config_package": runtime_config_package, + "joystick_config": "joystick.yaml", + "teleop_twist_config": "teleop_twist.yaml", + "use_sim": use_sim, + }.items(), + ) + + hardware = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "launch", + "hardware.launch.py" + ]) + ]), + launch_arguments={ + "use_sim": use_sim, + "robot_name": "rover", + "spawn_x": "0.0", + "spawn_y": "0.0", + "spawn_z": "3.0", + "spawn_yaw": "0.0", + }.items(), + ) + + controllers = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "launch", + "controllers.launch.py" + ]) + ]), + launch_arguments={ + "robot_controller": robot_controller, + "use_sim": use_sim, + "runtime_config_package": runtime_config_package, + }.items(), + ) + + visualization = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "launch", + "rviz.launch.py" + ]) + ]), + launch_arguments={ + "description_package": description_package, + "rviz_file": rviz_file, + "use_sim": use_sim, + }.items(), + condition=IfCondition(start_rviz), + ) + + return LaunchDescription([ + # Launch arguments + use_sim_arg, + runtime_config_package_arg, + description_package_arg, + description_file_arg, + rviz_file_arg, + prefix_arg, + robot_controller_arg, + start_rviz_arg, + world_arg, + # Launch files and nodes + sim_bringup, + robot_description, + teleop, + hardware, + controllers, + visualization, + ]) \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/launch/controllers.launch.py b/src/subsystems/drive/drive_bringup/launch/controllers.launch.py new file mode 100644 index 0000000..0181660 --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/controllers.launch.py @@ -0,0 +1,207 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def controller_spawning_logic(context, *args, **kwargs): + robot_controller = LaunchConfiguration("robot_controller").perform(context) + use_sim = LaunchConfiguration("use_sim") + switcher_delay = float(LaunchConfiguration("controller_switcher_delay").perform(context)) + runtime_config_package = LaunchConfiguration("runtime_config_package") + control_node_startup_delay = LaunchConfiguration("control_node_startup_delay") + + robot_controllers_path = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", "athena_drive_controllers.yaml"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + robot_controllers_path, + {"use_sim_time": use_sim} + ], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + condition=UnlessCondition(use_sim), + ) + + cmd_vel_relay = Node( + package="topic_tools", + executable="relay", + name="cmd_vel_to_ackermann_relay", + arguments=[ + "/cmd_vel", + "/ackermann_steering_controller/reference", + "--ros-args", + "--log-level", + "fatal", + ], + parameters=[{"use_sim_time": use_sim}], + output="log", + ) + + joy_relay = Node( + package="topic_tools", + executable="relay", + name="joy_to_single_ackermann_relay", + arguments=[ + "/joy", + "/single_ackermann_controller/reference", + "--ros-args", + "--log-level", + "fatal", + ], + parameters=[{"use_sim_time": use_sim}], + output="log", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + controllers_to_spawn = [] + + controllers_to_spawn.append( + Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "-c", "/controller_manager"], + ) + ) + + for controller in [ + "ackermann_steering_controller", + "single_ackermann_controller", + "drive_velocity_controller", + "drive_position_controller", + ]: + if controller != robot_controller: + controllers_to_spawn.append( + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager", "--inactive"], + ) + ) + + spawn_actions = [joint_state_broadcaster_spawner] + previous_action = joint_state_broadcaster_spawner + + for spawner_node in controllers_to_spawn: + spawn_actions.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=previous_action, + on_exit=[spawner_node], + ) + ) + ) + previous_action = spawner_node + + controller_switcher_node = Node( + package="bringup", + executable="controller_switcher.py", + name="controller_switcher", + output="screen", + parameters=[{"use_sim_time": use_sim}], + ) + + spawn_actions.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=previous_action, + on_exit=[ + TimerAction( + period=switcher_delay, + actions=[controller_switcher_node], + ) + ], + ) + ) + ) + + delayed_controllers_real = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[ + TimerAction( + period=control_node_startup_delay, + actions=spawn_actions, + ), + ], + ), + condition=UnlessCondition(use_sim), + ) + + delayed_controllers_sim = TimerAction( + period=control_node_startup_delay, + actions=spawn_actions, + condition=IfCondition(use_sim), + ) + + return [ + control_node, + cmd_vel_relay, + joy_relay, + delayed_controllers_real, + delayed_controllers_sim, + ] + + +def generate_launch_description(): + robot_controller_arg = DeclareLaunchArgument( + "robot_controller", + default_value="single_ackermann_controller", + choices=["single_ackermann_controller", "ackermann_steering_controller"], + description="The primary robot controller to start as active", + ) + + use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation mode (automatically sets use_sim_time)", + ) + + controller_switcher_delay_arg = DeclareLaunchArgument( + "controller_switcher_delay", + default_value="3.0", + description="Delay in seconds before starting controller switcher after controllers are loaded", + ) + + runtime_config_package_arg = DeclareLaunchArgument( + "runtime_config_package", + default_value="drive_bringup", + description="Package with the controller configuration in 'config' folder", + ) + + control_node_startup_delay_arg = DeclareLaunchArgument( + "control_node_startup_delay", + default_value="5.0", + description="Delay in seconds before starting controllers", + ) + + controller_manager_setup = OpaqueFunction(function=controller_spawning_logic) + + return LaunchDescription([ + robot_controller_arg, + use_sim_arg, + controller_switcher_delay_arg, + runtime_config_package_arg, + control_node_startup_delay_arg, + controller_manager_setup, + ]) \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/launch/hardware.launch.py b/src/subsystems/drive/drive_bringup/launch/hardware.launch.py new file mode 100644 index 0000000..77e1e72 --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/hardware.launch.py @@ -0,0 +1,59 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation (spawns robot in Gazebo) or real hardware (launches CAN node).", + ) + + robot_name_arg = DeclareLaunchArgument( + "robot_name", + default_value="rover", + description="Name of the robot in Gazebo.", + ) + + spawn_args = [ + DeclareLaunchArgument("spawn_x", default_value="0.0", description="Spawn X"), + DeclareLaunchArgument("spawn_y", default_value="0.0", description="Spawn Y"), + DeclareLaunchArgument("spawn_z", default_value="3.0", description="Spawn Z"), + DeclareLaunchArgument("spawn_yaw", default_value="0.0", description="Spawn Yaw"), + ] + + use_sim = LaunchConfiguration("use_sim") + robot_name = LaunchConfiguration("robot_name") + spawn_x = LaunchConfiguration("spawn_x") + spawn_y = LaunchConfiguration("spawn_y") + spawn_z = LaunchConfiguration("spawn_z") + spawn_yaw = LaunchConfiguration("spawn_yaw") + + + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", robot_name, + "-x", spawn_x, + "-y", spawn_y, + "-z", spawn_z, + "-Y", spawn_yaw, + "-topic", "robot_description", + ], + output="screen", + condition=IfCondition(use_sim) + ) + + return LaunchDescription( + [ + use_sim_arg, + robot_name_arg, + *spawn_args, + spawn_robot, + ] + ) \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/launch/robot_description.launch.py b/src/subsystems/drive/drive_bringup/launch/robot_description.launch.py new file mode 100644 index 0000000..8febbc5 --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/robot_description.launch.py @@ -0,0 +1,74 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "description_package", + default_value="description", + description="Description package with robot URDF/xacro files.", + ), + DeclareLaunchArgument( + "description_file", + default_value="athena_drive.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for multi-robot setup.", + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation mode (Gazebo) vs real hardware.", + ), + DeclareLaunchArgument( + "simulation_controllers", + default_value="", + description="Path to simulation controllers configuration file.", + ), + ] + + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_sim = LaunchConfiguration("use_sim") + simulation_controllers = LaunchConfiguration("simulation_controllers") + + robot_description_path = PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + robot_description_path, + " ", + "prefix:=", + prefix, + " ", + "use_sim:=", + use_sim, + " ", + "simulation_controllers:=", + simulation_controllers, + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description, {"use_sim_time": use_sim}], + ) + + return LaunchDescription(declared_arguments + [robot_state_pub_node]) diff --git a/src/subsystems/drive/drive_bringup/launch/rviz.launch.py b/src/subsystems/drive/drive_bringup/launch/rviz.launch.py new file mode 100644 index 0000000..42b691c --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/rviz.launch.py @@ -0,0 +1,57 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "description_package", + default_value="description", + description="Description package with robot URDF/xacro files.", + ), + DeclareLaunchArgument( + "rviz_file", + default_value="athena_drive.rviz", + description="RViz config file.", + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation mode (automatically sets use_sim_time).", + ), + DeclareLaunchArgument( + "startup_delay", + default_value="2.0", + description="Delay in seconds before starting RViz.", + ), + ] + + description_package = LaunchConfiguration("description_package") + rviz_file = LaunchConfiguration("rviz_file") + use_sim = LaunchConfiguration("use_sim") + startup_delay = LaunchConfiguration("startup_delay") + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", rviz_file] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[{"use_sim_time": use_sim}], + ) + + # Delay RViz startup to allow other nodes to initialize + delayed_rviz_launch = TimerAction( + period=startup_delay, + actions=[rviz_node], + ) + + return LaunchDescription(declared_arguments + [delayed_rviz_launch]) diff --git a/src/subsystems/drive/drive_bringup/launch/teleop.launch.py b/src/subsystems/drive/drive_bringup/launch/teleop.launch.py new file mode 100644 index 0000000..fece99e --- /dev/null +++ b/src/subsystems/drive/drive_bringup/launch/teleop.launch.py @@ -0,0 +1,79 @@ +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(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="drive_bringup", + description='Package with the 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.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim", + default_value="false", + choices=["true", "false"], + description="Use simulation mode (automatically sets use_sim_time).", + ) + ) + + runtime_config_package = LaunchConfiguration("runtime_config_package") + joystick_config = LaunchConfiguration("joystick_config") + teleop_twist_config = LaunchConfiguration("teleop_twist_config") + use_sim = LaunchConfiguration("use_sim") + + joystick_config_path = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", joystick_config] + ) + teleop_twist_config_path = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", teleop_twist_config] + ) + + joystick_publisher = Node( + package="teleop", + executable="joystick", + name="joystick", + output="screen", + parameters=[ + joystick_config_path, + {"use_sim_time": use_sim} + ], + remappings=[ + ("controller_input", "joy"), + ], + ) + + teleop_twist_joy = Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_twist_joy", + output="screen", + parameters=[ + teleop_twist_config_path, + {"use_sim_time": use_sim} + ], + ) + + return LaunchDescription( + declared_arguments + [joystick_publisher, teleop_twist_joy] + ) diff --git a/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py b/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py index 15d4832..98b297b 100644 --- a/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py @@ -1,24 +1,3 @@ -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node diff --git a/src/subsystems/drive/drive_bringup/package.xml b/src/subsystems/drive/drive_bringup/package.xml index 4c0ce17..c234d41 100644 --- a/src/subsystems/drive/drive_bringup/package.xml +++ b/src/subsystems/drive/drive_bringup/package.xml @@ -1,31 +1,39 @@ - drive_bringup - 0.0.0 - Athena - Ishan Dutta - Apache-2.0 + drive_bringup + 0.0.0 + Athena + Ishan Dutta + Apache-2.0 + ament_cmake - ament_cmake - - - hardware_interface - controller_manager - forward_command_controller - joint_state_broadcaster - ackermann_steering_controller - robot_state_publisher - ros2_controllers_test_nodes - teleop_twist_joy - rviz2 - xacro - rosidl_default_runtime - builtin_interfaces - - builtin_interfaces - - teleop + hardware_interface + controller_manager + forward_command_controller + joint_state_broadcaster + ackermann_steering_controller + robot_state_publisher + ros2_controllers_test_nodes + teleop_twist_joy + rviz2 + xacro + rosidl_default_runtime + builtin_interfaces + description + gz_ros2_control + ros2_control + ros2_controllers + odrive_ros2_control + rmd_ros2_control + bringup + builtin_interfaces + teleop + python3-pygame + python3-pygame + simulation + drive_controllers + topic_tools ament_cmake diff --git a/src/subsystems/drive/drive_bringup/resource/drive_bringup b/src/subsystems/drive/drive_bringup/resource/drive_bringup new file mode 100644 index 0000000..e69de29 diff --git a/src/subsystems/drive/drive_bringup/scripts/controller_switcher.py b/src/subsystems/drive/drive_bringup/scripts/controller_switcher.py deleted file mode 100755 index 44f141f..0000000 --- a/src/subsystems/drive/drive_bringup/scripts/controller_switcher.py +++ /dev/null @@ -1,231 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from rclpy.duration import Duration -import threading - -from msgs.srv import SetController -from controller_manager_msgs.srv import SwitchController, ListControllers - - -class ControllerSwitcher(Node): - def __init__(self): - super().__init__('controller_switcher') - - # Create separate callback groups - self.service_cb_group = ReentrantCallbackGroup() - self.client_cb_group = MutuallyExclusiveCallbackGroup() - - # Controllers to always keep active or ignore - self.always_active = ["joint_state_broadcaster"] - self.ignore_controllers = [] - - # Lock to prevent concurrent service processing - self.service_lock = threading.Lock() - - # Create service with callback group - self.srv = self.create_service( - SetController, - 'set_controller', - self.set_controller_callback, - callback_group=self.service_cb_group - ) - - # Create clients with separate callback group - self.switch_client = self.create_client( - SwitchController, - '/controller_manager/switch_controller', - callback_group=self.client_cb_group - ) - self.list_client = self.create_client( - ListControllers, - '/controller_manager/list_controllers', - callback_group=self.client_cb_group - ) - - # Wait for services with timeout - self.get_logger().info('Waiting for controller_manager services...') - services_ready = False - for _ in range(30): # Try for 30 seconds - if (self.switch_client.wait_for_service(timeout_sec=1.0) and - self.list_client.wait_for_service(timeout_sec=1.0)): - services_ready = True - break - self.get_logger().warn('Still waiting for controller_manager services...') - - if not services_ready: - self.get_logger().error('Controller manager services not available!') - raise RuntimeError('Controller manager services timeout') - - self.get_logger().info('ControllerSwitcher ready!') - - def set_controller_callback(self, request, response): - # Use lock to ensure only one service call is processed at a time - with self.service_lock: - return self._handle_set_controller(request, response) - - def _handle_set_controller(self, request, response): - try: - requested_controllers = list(request.controller_names) if request.controller_names else [] - - # Empty list means deactivate all (except always_active and ignored) - if not requested_controllers: - self.get_logger().info("Empty controller list - will deactivate all except protected controllers") - else: - self.get_logger().info(f"Received request to switch to: {requested_controllers}") - - # Get list of all controllers - list_req = ListControllers.Request() - self.get_logger().debug("Calling list_controllers service...") - - # Call and wait for response - try: - list_result = self.list_client.call(list_req) - except Exception as e: - response.success = False - response.message = f"Failed to call list_controllers: {str(e)}" - return response - - if list_result is None: - response.success = False - response.message = "Failed to get controller list" - return response - - # Extract controller names and their states - all_controllers = [] - active_controllers = [] - for c in list_result.controller: - all_controllers.append(c.name) - if c.state == 'active': - active_controllers.append(c.name) - - self.get_logger().info(f"Available controllers: {all_controllers}") - self.get_logger().info(f"Currently active: {active_controllers}") - - # Check if all requested controllers exist (only if not empty) - if requested_controllers: - missing_controllers = [] - for controller in requested_controllers: - if controller not in all_controllers: - missing_controllers.append(controller) - - if missing_controllers: - response.success = False - response.message = f"Controllers not found: {missing_controllers}. Available: {all_controllers}" - return response - - # Build lists for activation/deactivation - controllers_to_activate = [] - - # If not empty, add requested controllers that aren't already active - if requested_controllers: - for controller in requested_controllers: - if controller not in active_controllers: - controllers_to_activate.append(controller) - - # Always ensure joint_state_broadcaster is active - if "joint_state_broadcaster" not in active_controllers and "joint_state_broadcaster" not in controllers_to_activate: - controllers_to_activate.append("joint_state_broadcaster") - - # Deactivate logic: - # - If empty list: deactivate all except always_active and ignored - # - If controllers specified: deactivate all except requested, always_active, and ignored - controllers_to_deactivate = [] - for controller in active_controllers: - if controller in self.always_active or controller in self.ignore_controllers: - continue # Skip protected controllers - - if not requested_controllers or controller not in requested_controllers: - controllers_to_deactivate.append(controller) - - # Check if already in desired state - if requested_controllers: - all_requested_active = all(c in active_controllers for c in requested_controllers) - if all_requested_active and len(controllers_to_deactivate) == 0: - response.success = True - response.message = f"Controllers {requested_controllers} already active with correct configuration" - self.get_logger().info("Already in desired state") - return response - else: - # For empty list, check if only protected controllers are active - if len(controllers_to_deactivate) == 0 and len(controllers_to_activate) == 0: - response.success = True - response.message = "Only protected controllers are active" - self.get_logger().info("Already in desired state") - return response - - self.get_logger().info(f"Will activate: {controllers_to_activate}") - self.get_logger().info(f"Will deactivate: {controllers_to_deactivate}") - - # Skip if nothing to do - if not controllers_to_activate and not controllers_to_deactivate: - response.success = True - response.message = "No changes needed" - return response - - # Perform the switch - switch_req = SwitchController.Request() - switch_req.activate_controllers = controllers_to_activate - switch_req.deactivate_controllers = controllers_to_deactivate - switch_req.strictness = SwitchController.Request.BEST_EFFORT - switch_req.start_asap = True - switch_req.timeout = Duration(seconds=10).to_msg() - - self.get_logger().debug("Calling switch_controller service...") - - try: - switch_result = self.switch_client.call(switch_req) - except Exception as e: - response.success = False - response.message = f"Failed to call switch_controller: {str(e)}" - return response - - if switch_result and switch_result.ok: - response.success = True - if requested_controllers: - active_list = requested_controllers + self.always_active - response.message = f"Successfully activated controllers: {requested_controllers}. Active: {active_list}" - else: - response.message = f"Successfully deactivated all controllers except: {self.always_active + self.ignore_controllers}" - self.get_logger().info(f"Success! {response.message}") - else: - response.success = False - response.message = "Controller switch failed" - self.get_logger().error("Switch failed") - - except Exception as e: - self.get_logger().error(f"Error in set_controller_callback: {str(e)}") - import traceback - self.get_logger().error(traceback.format_exc()) - response.success = False - response.message = f"Exception: {str(e)}" - - self.get_logger().info(f"Returning response: success={response.success}, message={response.message}") - return response - - -def main(args=None): - rclpy.init(args=args) - - try: - node = ControllerSwitcher() - # Use MultiThreadedExecutor with more threads - executor = MultiThreadedExecutor(num_threads=4) - executor.add_node(node) - - try: - executor.spin() - finally: - executor.shutdown() - node.destroy_node() - - except Exception as e: - print(f"Failed to start ControllerSwitcher: {e}") - finally: - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py index 3b9abba..be104e7 100644 --- a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py +++ b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py @@ -48,7 +48,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + #self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 18f1ce7..908acb8 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -57,7 +57,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + #self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: