SW Launch - Weber-State-Submarine-Project/Submarine GitHub Wiki

Introduction

This section documents the ROS 2 launch file used to initialize and manage the nodes for the autonomous boat system. The launch file ensures that all required nodes and components are launched in a coordinated manner.


Overview of the Launch File

The launch file initializes multiple nodes and includes other launch files required for specific functionalities. Key responsibilities include:

  • Launching sensor and controller nodes.
  • Managing robot state publishers and localization nodes.
  • Integrating external tools like Foxglove for visualization.

Key Components

Included Launch Files

  • Foxglove Bridge:
    Initializes the Foxglove Bridge for WebSocket-based visualization and monitoring.
  • SLAM Launch:
    Includes the SLAM (Simultaneous Localization and Mapping) launch file for online asynchronous mapping.
  • Octomap Mapping:
    Includes the Octomap launch file for 3D occupancy mapping.

Nodes

  • State Publisher (robot_state_publisher):
    Publishes the robot's state using an SDF model file.
  • Sonar Nodes:
    • down_sonar_node: Processes downward-facing sonar data.
    • side_sonar_node: Processes side-facing sonar data.
    • front_sonar_node: Processes front-facing sonar data.
  • Speed Sensor (ping_velocity):
    Publishes velocity data from the speed sensor to the /odom topic.
  • Orientation Nodes:
    • orientation_publisher: Publishes raw orientation data.
    • imu: Converts orientation data into the standard sensor_msgs/Imu format.
  • Motor Control Nodes:
    • esc_subscriber: Handles ESC motor control.
    • joy_controller: Translates joystick commands to ESC commands.
  • Joystick Node (joy_node):
    Publishes joystick inputs for motor control and navigation.
  • Additional Nodes (Optional):
    • pose_node: Publishes robot pose data.
    • lidar: Integrates LIDAR sensor data for additional mapping.

Challenges

  1. Complex Integration:
    Managing multiple dependencies and ensuring all nodes work seamlessly required careful configuration and debugging.
  2. Performance Optimization:
    Balancing computational loads across nodes to avoid overloading the system was critical for reliable operation.
  3. Topic Coordination:
    Ensuring proper topic names and parameter configurations to avoid conflicts between nodes.

Launch File

Below is the original launch file as implemented:

from launch import LaunchDescription
from launch_ros.actions import Node,SetParameter
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource, XMLLaunchDescriptionSource
import os

def generate_launch_description():
    sub_path = get_package_share_directory('ros2_sub')

    octo = Node(
        package='ros2_sub',
        executable='octo',
        name='octo',
        output='screen'
    )

    foxglove_bridge = IncludeLaunchDescription(
        XMLLaunchDescriptionSource(
            os.path.join(get_package_share_directory('foxglove_bridge'), 'launch', 'foxglove_bridge_launch.xml')
        ),
        launch_arguments={'port': '8765'}.items()
    ) 
    
    ros_bt = Node(
        package='ros_bt',
        executable='sub_exec',
        name='exec_node',
        output='screen'
    )
    
    ekf = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[os.path.join(sub_path, 'config','ekf.yaml')]
    )

    slam = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
        os.path.join(get_package_share_directory('ros2_sub'),'launch','online_async_launch.py')),
    )


    nav2 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
        os.path.join(get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py')),
    )

    sdf_file = os.path.join(sub_path, 'models', 'tethys', 'model.sdf')
    with open(sdf_file, 'r') as infp:
        sub_desc = infp.read()

    sub_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='both',
        parameters=[
            {'robot_description':sub_desc},
        ]
    )

    octomap = IncludeLaunchDescription(
        XMLLaunchDescriptionSource(
            os.path.join(get_package_share_directory('ros2_sub'), 'launch', 'octomap_mapping.launch.xml')),
    )

    #/scan/down
    #/scan/down/points
    down_sonar = Node(
        package='sonars',
        executable='down_sonar_node',
        name='down_sonar',
        output='screen',
        )
    
    #/scan/left
    #/scan/left/points
    side_sonar = Node(
        package='sonars',
        executable='side_sonar_node',
        name='side_sonar',
        output='screen',
        )

    #/scan/front
    #/scan/front/points
    front_sonar = Node(
        package='sonars',
        executable='front_sonar_node',
        name='front_sonar',
        output='screen',
        )

    #/odom
    speed_sensor = Node(
        package='speed_sensor',
        executable='ping_velocity',
        name='speed_sensor',
        output='screen',
        )

    orientation= Node(
        package='orientation_package',
        executable='orientation_publisher',
        name='orientation',
        output='screen',
        )

    #/imu
    imu = Node(
        package='orientation_package',
        executable='imu',
        name='imu',
        output='screen',
        )

    #add motor controller here
    motor_control = Node(
        package='esc_pkg',
        executable='esc_subscriber',
        name='motor_control',
        output='screen',
        )
    
    #Publishes commands to motors
    joy_to_esc = Node(
        package='joy_controller_to_esc',
        executable='joy_controller',
        name='joy_to_esc',
        output='screen',
        )

    #listens to controller inputs
    joy = Node(
        package='joy',
        executable='joy_node',
        name='joy_node',
        output='screen',
        )

    pose = Node(
        package='ros2_sub',
        executable='pose',
        name='pose_node',
        output='screen',
        )


    lidar = Node(
        package='speed_sensor',
        executable='lidar',
        name='lidar',
        output='screen',
        )

    return LaunchDescription([
        sub_state_publisher,
        slam,
        ekf,
        ros_bt,
        octomap,
        octo,
        #nav2,
        foxglove_bridge,
        down_sonar,
        side_sonar,
        front_sonar,
        speed_sensor,
        orientation,
        imu,
        motor_control,
        joy_to_esc,
        joy,
        #pose
        #lidar
        ])