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 standardsensor_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
- Complex Integration:
Managing multiple dependencies and ensuring all nodes work seamlessly required careful configuration and debugging. - Performance Optimization:
Balancing computational loads across nodes to avoid overloading the system was critical for reliable operation. - 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
])