diff --git a/launch/launch_hw.launch.py b/launch/launch_hw.launch.py index b87c276..0058eb8 100644 --- a/launch/launch_hw.launch.py +++ b/launch/launch_hw.launch.py @@ -14,6 +14,8 @@ def generate_launch_description(): ecu_port = LaunchConfiguration('ecu_port') rpi_port = LaunchConfiguration('rpi_port') + log_level = LaunchConfiguration('log_level', default='info') + ld = LaunchDescription([ DeclareLaunchArgument( 'ecu_ip', @@ -32,6 +34,14 @@ def generate_launch_description(): ) ]) + log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' + ) + + ld.add_action(log_level_arg) + # Package paths package_name = 'kpi_rover' pkg_share = get_package_share_directory(package_name) @@ -59,6 +69,7 @@ def generate_launch_description(): package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': robot_description}], + arguments=['--ros-args', '--log-level', log_level], output='screen' )) @@ -73,6 +84,7 @@ def generate_launch_description(): {'ecu_port': ecu_port}, {'rpi_port': rpi_port} ], + arguments=['--ros-args', '--log-level', log_level], output='screen' ) ld.add_action(controller_manager) @@ -84,19 +96,19 @@ def generate_launch_description(): Node( package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster'], + arguments=['joint_state_broadcaster', '--ros-args', '--log-level', log_level], output='screen' ), Node( package='controller_manager', executable='spawner', - arguments=['diff_drive_controller'], + arguments=['diff_drive_controller', '--ros-args', '--log-level', log_level], output='screen' ), Node( package="controller_manager", executable="spawner", - arguments=["imu_broadcaster"], + arguments=["imu_broadcaster", '--ros-args', '--log-level', log_level], ) ] ) diff --git a/launch/launch_irl_base_station.launch.py b/launch/launch_irl_base_station.launch.py new file mode 100644 index 0000000..f1cefe6 --- /dev/null +++ b/launch/launch_irl_base_station.launch.py @@ -0,0 +1,86 @@ +""" +Launch file for the KPIRover running on real hardware (RPI, lidar, camera, motors with encoders). + + +This file launches the following components: +- ROS2 control system for motors with joystick control. +- EKF node for sensor fusion and localization. +- SLAM toolbox for online asynchronous mapping. +- Navigation stack for path planning. + + +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +# Define the common clock parameter (not using simulation) +use_sim_time = LaunchConfiguration('use_sim_time', default='false') + +log_level = LaunchConfiguration('log_level', default='info') + +package_name = 'kpi_rover' + +ld = LaunchDescription() +ld.add_action(DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true')) + +log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' +) + +ld.add_action(log_level_arg) + +# Launch the EKF node for sensor fusion and localization. +# It fuses sensor data (e.g., IMU, odometry) to estimate the robot's pose. +ekf = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[ + os.path.join(get_package_share_directory(package_name), 'config', 'ekf.yaml'), + {'use_sim_time': use_sim_time}, + ], + arguments=['--ros-args', '--log-level', log_level] +) + +# Launch the SLAM toolbox for online asynchronous mapping. +# It builds a map of the environment from sensor data. +slam_toolbox_map = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py') + ), + launch_arguments={ + 'slam_params_file': os.path.join(get_package_share_directory(package_name), 'config', 'slam_toolbox_mapping.yaml'), + 'use_sim_time': use_sim_time, + 'log_level': log_level, + }.items() +) + +# Launch the navigation stack. +# Provides path planning and obstacle avoidance for autonomous robot movement. +nav = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package_name), 'launch', 'navigation.launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'log_level': log_level, + }.items() +) + +# Add all components into the LaunchDescription in the desired sequence. + +ld.add_action(ekf) # Run EKF for sensor fusion and localization. +ld.add_action(slam_toolbox_map) # Run SLAM toolkit for mapping. +ld.add_action(nav) # Start navigation stack. +def generate_launch_description(): + return ld diff --git a/launch/launch_irl.launch.py b/launch/launch_irl_rpi.launch.py similarity index 65% rename from launch/launch_irl.launch.py rename to launch/launch_irl_rpi.launch.py index b930925..5bc98cc 100644 --- a/launch/launch_irl.launch.py +++ b/launch/launch_irl_rpi.launch.py @@ -28,11 +28,13 @@ ecu_port = LaunchConfiguration('ecu_port') udp_port = LaunchConfiguration('udp_port') +log_level = LaunchConfiguration('log_level', default='info') + package_name = 'kpi_rover' ld = LaunchDescription() ld.add_action(DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true')) -ld = LaunchDescription([ +ld.add_action(LaunchDescription([ DeclareLaunchArgument( 'ecu_ip', default_value='10.30.30.30', @@ -49,12 +51,24 @@ description='Port number of the UDP server to listen for IMU data from ECU' ) ]) +) + +log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' +) + +ld.add_action(log_level_arg) # Launch lidar node from cspc_lidar package lidar = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('cspc_lidar'), 'launch', 'lidar.launch.py') - ) + ), + launch_arguments={ + 'log_level': log_level + }.items() ) # Launch ros2_control system for driving real motors motors_control = IncludeLaunchDescription( @@ -65,44 +79,11 @@ 'use_sim_time': use_sim_time, 'ecu_ip': ecu_ip, 'ecu_port': ecu_port, - 'udp_port': udp_port - }.items() -) - -# Launch the EKF node for sensor fusion and localization. -# It fuses sensor data (e.g., IMU, odometry) to estimate the robot's pose. -ekf = Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node', - output='screen', - parameters=[ - os.path.join(get_package_share_directory(package_name), 'config', 'ekf.yaml'), - {'use_sim_time': use_sim_time}, - ] -) - -# Launch the SLAM toolbox for online asynchronous mapping. -# It builds a map of the environment from sensor data. -slam_toolbox_map = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py') - ), - launch_arguments={ - 'slam_params_file': os.path.join(get_package_share_directory(package_name), 'config', 'slam_toolbox_mapping.yaml'), - 'use_sim_time': use_sim_time + 'udp_port': udp_port, + 'log_level': log_level, }.items() ) -# Launch the navigation stack. -# Provides path planning and obstacle avoidance for autonomous robot movement. -nav = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory(package_name), 'launch', 'navigation.launch.py') - ), - launch_arguments={'use_sim_time': use_sim_time}.items() -) - # Launch camera node. camera = Node( package='v4l2_camera', @@ -113,6 +94,7 @@ 'image_size': [320, 240], 'video_device':"/dev/video0", 'output_encoding': "yuv422_yuy2"}], + arguments=['--ros-args', '--log-level', log_level], remappings=[('/image_raw','/camera/image_raw')] ) @@ -120,10 +102,7 @@ # Add all components into the LaunchDescription in the desired sequence. ld.add_action(motors_control) # Start all nodes for motors control. -ld.add_action(ekf) # Run EKF for sensor fusion and localization. ld.add_action(lidar) # Run lidar node -ld.add_action(slam_toolbox_map) # Run SLAM toolkit for mapping. -ld.add_action(nav) # Start navigation stack. ld.add_action(camera) # Start publishing images from camera. def generate_launch_description(): return ld diff --git a/launch/launch_sim.launch.py b/launch/launch_sim.launch.py index 3abef0c..f3ac895 100644 --- a/launch/launch_sim.launch.py +++ b/launch/launch_sim.launch.py @@ -26,12 +26,22 @@ # Define the common simulation clock parameter use_sim_time = LaunchConfiguration('use_sim_time', default='true') +log_level = LaunchConfiguration('log_level', default='info') + package_name = 'kpi_rover' ros_gz_sim = get_package_share_directory('ros_gz_sim') ld = LaunchDescription() ld.add_action(DeclareLaunchArgument('use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true')) +log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' +) + +ld.add_action(log_level_arg) + # Launch Gazebo simulation server with the specified world file. # This starts the simulation environment. world = os.path.join( @@ -45,7 +55,8 @@ ), launch_arguments={ 'gz_args': ['-r -s -v4 ', world], - 'on_exit_shutdown': 'true' + 'on_exit_shutdown': 'true', + 'log_level': log_level, }.items() ) @@ -57,7 +68,8 @@ ), launch_arguments={ 'use_sim_time': use_sim_time, - 'use_ros2_control': 'true' + 'use_ros2_control': 'true', + 'log_level': log_level, }.items() ) @@ -70,21 +82,22 @@ launch_arguments={ 'x_pose': LaunchConfiguration('x_pose', default='0'), 'y_pose': LaunchConfiguration('y_pose', default='0'), - 'z_pose': LaunchConfiguration('z_pose', default='0') + 'z_pose': LaunchConfiguration('z_pose', default='0'), + 'log_level': log_level, }.items() ) # Load the Joint State Broadcaster controller. # This controller publishes the joint states necessary for robot control. load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster', '--ros-args', '--log-level', log_level], output='screen' ) # Load the Differential Drive controller. # This controller converts velocity commands into motor commands for robot maneuvering. load_diff_drive_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'diff_drive_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'diff_drive_controller', '--ros-args', '--log-level', log_level], output='screen' ) @@ -98,7 +111,8 @@ parameters=[ os.path.join(get_package_share_directory(package_name), 'config', 'ekf.yaml'), {'use_sim_time': use_sim_time}, - ] + ], + arguments=['--ros-args', '--log-level', log_level] ) # Launch the SLAM toolbox for online asynchronous mapping. @@ -109,7 +123,8 @@ ), launch_arguments={ 'slam_params_file': os.path.join(get_package_share_directory(package_name), 'config', 'slam_toolbox_mapping.yaml'), - 'use_sim_time': use_sim_time + 'use_sim_time': use_sim_time, + 'log_level': log_level, }.items() ) @@ -119,7 +134,10 @@ PythonLaunchDescriptionSource( os.path.join(get_package_share_directory(package_name), 'launch', 'navigation.launch.py') ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={ + 'use_sim_time': use_sim_time, + 'log_level': log_level, + }.items() ) # Include the Gazebo UI . @@ -128,7 +146,10 @@ PythonLaunchDescriptionSource( os.path.join(get_package_share_directory(package_name), 'launch', 'launch_sim_ui.launch.py') ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={ + 'use_sim_time': use_sim_time, + 'log_level': log_level, + }.items() ) # Add all components into the LaunchDescription in the desired sequence. diff --git a/launch/launch_sim_ui.launch.py b/launch/launch_sim_ui.launch.py index 31a6536..1159932 100644 --- a/launch/launch_sim_ui.launch.py +++ b/launch/launch_sim_ui.launch.py @@ -24,13 +24,24 @@ # Declare and initialize the use_sim_time parameter use_sim_time = LaunchConfiguration('use_sim_time', default='true') +log_level = LaunchConfiguration('log_level', default='info') + +log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' +) + # Launch the Gazebo client UI # This node provides the simulation UI for Gazebo. gzclient_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') ), - launch_arguments={'gz_args': '-g -v4 '}.items() + launch_arguments={ + 'gz_args': '-g -v4 ', + 'log_level': log_level, + }.items() ) # Launch RViz2 with use_sim_time parameter passed in its configuration @@ -39,7 +50,10 @@ package='rviz2', executable='rviz2', name='rviz2', - arguments=['-d', os.path.join(get_package_share_directory(package_name), 'config', 'view_bot.rviz')], + arguments=[ + '-d', os.path.join(get_package_share_directory(package_name), 'config', 'view_bot.rviz'), + '--ros-args', '--log-level', log_level, + ], parameters=[{'use_sim_time': use_sim_time}], output='screen' ) @@ -47,6 +61,7 @@ # Create and populate the LaunchDescription. ld = LaunchDescription() ld.add_action(DeclareLaunchArgument('use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true')) +ld.add_action(log_level_arg) ld.add_action(gzclient_cmd) ld.add_action(rviz) diff --git a/launch/navigation.launch.py b/launch/navigation.launch.py index 917b9d9..5e3b3f7 100644 --- a/launch/navigation.launch.py +++ b/launch/navigation.launch.py @@ -16,6 +16,14 @@ def generate_launch_description(): description='Flag to enable use_sim_time' ) + log_level = LaunchConfiguration('log_level', default='info') + + log_level_arg = DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Logging level (debug, info, warn, error, fatal)' + ) + nav2_navigation_launch_path = os.path.join( get_package_share_directory('nav2_bringup'), 'launch', @@ -34,6 +42,7 @@ def generate_launch_description(): launch_arguments={ 'use_sim_time': LaunchConfiguration('use_sim_time'), 'params_file': navigation_params_path, + 'log_level': log_level, }.items() ) @@ -41,6 +50,7 @@ def generate_launch_description(): package='twist_stamper', executable='twist_stamper', parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + arguments=['--ros-args', '--log-level', log_level], remappings=[('/cmd_vel_in','/cmd_vel_unstamped'), ('/cmd_vel_out','/diff_drive_controller/cmd_vel')] ) @@ -49,6 +59,7 @@ def generate_launch_description(): launchDescriptionObject = LaunchDescription() launchDescriptionObject.add_action(sim_time_arg) + launchDescriptionObject.add_action(log_level_arg) launchDescriptionObject.add_action(navigation_launch) launchDescriptionObject.add_action(twist_stamper)