Merge branch 'develop' into develop-humble

.github/workflows/auto-merge-workflow.yml

@@ -0,0 +1,14 @@
+name: Auto Merge develop to develop-humble
+
+on:
+  schedule:
+    - cron: '0 0 * * 1'  # Weekly on Monday
+  workflow_dispatch:  
+
+jobs:
+  call-merge-workflow:
+    uses: usdot-fhwa-stol/actions/.github/workflows/auto-pr-merge.yml@main
+    with:
+      branch_from: develop
+      branch_to: develop-humble
+      notify_team: DevOps

carma-messenger-core/carma-messenger/launch/carma-messenger.launch.py

@@ -30,102 +30,138 @@
 
 import os
 
+
 def generate_launch_description():
     """
     Launch CARMA Messenger System.
     """
 
-    configuration_delay = LaunchConfiguration('configuration_delay')
+    configuration_delay = LaunchConfiguration("configuration_delay")
     declare_configuration_delay_arg = DeclareLaunchArgument(
-        name ='configuration_delay', default_value='4.0')
-
-    use_rosbag = LaunchConfiguration('use_rosbag')
+        name="configuration_delay", default_value="4.0"
+    )
+
+    use_rosbag = LaunchConfiguration("use_rosbag")
     declare_use_rosbag = DeclareLaunchArgument(
-        name = 'use_rosbag',
-        default_value = 'true',
-        description = "Record a ROS2 bag"
+        name="use_rosbag", default_value="true", description="Record a ROS2 bag"
     )
 
     # Declare the route file folder launch argument
-    route_file_folder = LaunchConfiguration('route_file_folder')
+    route_file_folder = LaunchConfiguration("route_file_folder")
     declare_route_file_folder = DeclareLaunchArgument(
-        name = 'route_file_folder',
-        default_value='/opt/carma/routes/',
-        description = 'Path of folder on host PC containing route CSV file(s) that can be accessed by plugins'
+        name="route_file_folder",
+        default_value="/opt/carma/routes/",
+        description="Path of folder on host PC containing route CSV file(s) that can be accessed by plugins",
+    )
+
+    # Launch ROS2 rosbag logging
+    ros2_rosbag_launch = GroupAction(
+        actions=[
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    [ThisLaunchFileDir(), "/ros2_rosbag.launch.py"]
+                ),
+            )
+        ]
     )
 
     transform_group = GroupAction(
         actions=[
-            PushRosNamespace(EnvironmentVariable('CARMA_TF_NS', default_value='/')),
+            PushRosNamespace(EnvironmentVariable("CARMA_TF_NS", default_value="/")),
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/transforms.launch.py']),
+                PythonLaunchDescriptionSource(
+                    [ThisLaunchFileDir(), "/transforms.launch.py"]
+                ),
             ),
         ]
     )
 
     v2x_group = GroupAction(
         actions=[
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/message.launch.py']),
-                launch_arguments = { 
-                    'configuration_delay' : [configuration_delay]
-                }.items()
+                PythonLaunchDescriptionSource(
+                    [ThisLaunchFileDir(), "/message.launch.py"]
+                ),
+                launch_arguments={"configuration_delay": [configuration_delay]}.items(),
             ),
         ]
     )
 
     plugins_group = GroupAction(
         actions=[
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/plugins.launch.py']),
-                launch_arguments = { 
-                    'configuration_delay' : [configuration_delay],
-                    'route_file_folder' : route_file_folder
-                }.items()
+                PythonLaunchDescriptionSource(
+                    [ThisLaunchFileDir(), "/plugins.launch.py"]
+                ),
+                launch_arguments={
+                    "configuration_delay": [configuration_delay],
+                    "route_file_folder": route_file_folder,
+                }.items(),
             ),
         ]
     )
 
     ui_group = GroupAction(
         actions=[
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ui.launch.py']),
+                PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/ui.launch.py"]),
             ),
         ]
     )
 
     # Launch ROS2 rosbag logging
     ros2_rosbag_group = GroupAction(
-        condition = IfCondition(use_rosbag),
+        condition=IfCondition(use_rosbag),
         actions=[
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([get_package_share_directory('carma-messenger'), '/launch', '/ros2_rosbag.launch.py']),
+                PythonLaunchDescriptionSource(
+                    [
+                        get_package_share_directory("carma-messenger"),
+                        "/launch",
+                        "/ros2_rosbag.launch.py",
+                    ]
+                ),
             )
-        ]
+        ],
     )
 
     traffic_incident_group = GroupAction(
         actions=[
             IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([get_package_share_directory('traffic_incident'), '/launch', '/traffic_incident.launch.py']),
+                PythonLaunchDescriptionSource(
+                    [
+                        get_package_share_directory("traffic_incident"),
+                        "/launch",
+                        "/traffic_incident.launch.py",
+                    ]
+                ),
             ),
         ]
     )
 
-
     system_alert_publisher = ExecuteProcess(
-        cmd=['ros2', 'topic', 'pub', '/system_alert', 'carma_msgs/msg/SystemAlert', '"{ type: 5, description: Simulated Drivers Ready }"']
+        cmd=[
+            "ros2",
+            "topic",
+            "pub",
+            "/system_alert",
+            "carma_msgs/msg/SystemAlert",
+            '"{ type: 5, description: Simulated Drivers Ready }"',
+        ]
     )
 
-    return LaunchDescription([
-        declare_configuration_delay_arg,
-        declare_use_rosbag,
-        declare_route_file_folder,
-        transform_group,
-        v2x_group,
-        plugins_group,
-        ui_group,
-        ros2_rosbag_group,
-        traffic_incident_group,
-        system_alert_publisher
-    ])
+    return LaunchDescription(
+        [
+            declare_configuration_delay_arg,
+            declare_use_rosbag,
+            declare_route_file_folder,
+            ros2_rosbag_launch,
+            transform_group,
+            v2x_group,
+            plugins_group,
+            ui_group,
+            ros2_rosbag_group,
+            traffic_incident_group,
+            system_alert_publisher,
+        ]
+    )

carma-messenger-core/carma-messenger/launch/ros2_rosbag.launch.py

@@ -1,4 +1,4 @@
-# Copyright (C) 2023 LEIDOS.
+# Copyright (C) 2024 LEIDOS.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -24,33 +24,57 @@
 import pathlib
 import yaml
 
+
 # This function is used to generate a command to record a ROS 2 rosbag that excludes topics
 # topics as provided in the appropriate configuration file.
 def record_ros2_rosbag(context: LaunchContext, rosbag2_qos_override_param_file):
 
-    overriding_qos_profiles = context.perform_substitution(rosbag2_qos_override_param_file)
+    overriding_qos_profiles = context.perform_substitution(
+        rosbag2_qos_override_param_file
+    )
 
     proc = ExecuteProcess(
-            cmd=['ros2', 'bag', 'record', '-s', 'mcap', '--qos-profile-overrides-path', overriding_qos_profiles, '-o', '/opt/carma/logs/rosbag2_' + str(datetime.now().strftime('%Y-%m-%d_%H%M%S')), '-a'],
-            output='screen',
-            shell='true'
-        )
+        cmd=[
+            "ros2",
+            "bag",
+            "record",
+            "-s",
+            "mcap",
+            "--qos-profile-overrides-path",
+            overriding_qos_profiles,
+            "-o",
+            "/opt/carma/logs/rosbag2_"
+            + str(datetime.now().strftime("%Y-%m-%d_%H%M%S")),
+            "-a",
+        ],
+        output="screen",
+        shell="true",
+    )
 
     return [proc]
 
 
 def generate_launch_description():
-    rosbag2_qos_override_param_file = LaunchConfiguration('rosbag2_qos_override_param_file')
+    rosbag2_qos_override_param_file = LaunchConfiguration(
+        "rosbag2_qos_override_param_file"
+    )
     declare_rosbag2_qos_override_param_file = DeclareLaunchArgument(
-        name='rosbag2_qos_override_param_file',
-        default_value = PathJoinSubstitution([
-                    FindPackageShare('carma-messenger'),'config',
-                    'rosbag2_qos_overrides.yaml'
-                ]),
-        description = "Path to file containing rosbag2 override qos settings"
+        name="rosbag2_qos_override_param_file",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("carma-messenger"),
+                "config",
+                "rosbag2_qos_overrides.yaml",
+            ]
+        ),
+        description="Path to file containing rosbag2 override qos settings",
     )
 
-    return LaunchDescription([
-        declare_rosbag2_qos_override_param_file,
-        OpaqueFunction(function=record_ros2_rosbag, args=[rosbag2_qos_override_param_file])
-    ])
+    return LaunchDescription(
+        [
+            declare_rosbag2_qos_override_param_file,
+            OpaqueFunction(
+                function=record_ros2_rosbag, args=[rosbag2_qos_override_param_file]
+            ),
+        ]
+    )

carma-messenger-core/traffic_incident/include/traffic_incident_node.hpp

@@ -86,6 +86,7 @@ class TrafficIncidentNode : public carma_ros2_utils::CarmaLifecycleNode
   double down_track_;
   double up_track_;
   double min_gap_;
+  static constexpr double epsilon_ = 1e-6;
 };
 
 }  // namespace traffic

carma-messenger-core/traffic_incident/src/traffic_incident_node.cpp

@@ -150,8 +150,7 @@ bool TrafficIncidentNode::stopTrafficBroadcastCallback(
 void TrafficIncidentNode::spin_callback(void)
 {
   if (
-    traffic_worker_.getDownTrack() > 0 && traffic_worker_.getUpTrack() > 0 &&
-    traffic_worker_.getMinGap() > 0 && traffic_worker_.getAdvisorySpeed() > 0) {
+    std::fabs(traffic_worker_.getDownTrack()) > epsilon_ || std::fabs(traffic_worker_.getUpTrack() > epsilon_) || std::fabs(traffic_worker_.getAdvisorySpeed() > epsilon_)) {
     // construct local mobilityOperation msg
     carma_v2x_msgs::msg::MobilityOperation traffic_mobility_msg =
       traffic_worker_.mobilityMessageGenerator(traffic_worker_.getPinPoint());