Merge remote-tracking branch 'awf/main' into fix-coonflict-awf-latest-sync-upstream

Author: TakaHoribe

.cppcheck_suppressions

@@ -1,7 +1,6 @@
 *:*/test/*
 
 checkersReport
-constParameterPointer
 constParameterReference
 constVariable
 constVariableReference
@@ -23,8 +22,6 @@ noValidConfiguration
 passedByValue
 preprocessorErrorDirective
 redundantInitialization
-returnByReference
-shadowArgument
 shadowFunction
 shadowVariable
 syntaxError

.github/CODEOWNERS

@@ -185,12 +185,12 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_mod
 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
+planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
-planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp

.github/PULL_REQUEST_TEMPLATE.md

@@ -0,0 +1,61 @@
+## Description
+
+## Related links
+
+**Parent Issue:**
+
+- Link
+
+<!-- ⬇️🟢
+**Private Links:**
+
+- [CompanyName internal link]()
+⬆️🟢 -->
+
+## How was this PR tested?
+
+## Notes for reviewers
+
+None.
+
+## Interface changes
+
+None.
+
+<!-- ⬇️🔴
+
+### Topic changes
+
+#### Additions and removals
+
+| Change type   | Topic Type      | Topic Name    | Message Type        | Description       |
+|:--------------|:----------------|:--------------|:--------------------|:------------------|
+| Added/Removed | Pub/Sub/Srv/Cli | `/topic_name` | `std_msgs/String`   | Topic description |
+
+#### Modifications
+
+| Version | Topic Type      | Topic Name        | Message Type        | Description       |
+|:--------|:----------------|:------------------|:--------------------|:------------------|
+| Old     | Pub/Sub/Srv/Cli | `/old_topic_name` | `sensor_msgs/Image` | Topic description |
+| New     | Pub/Sub/Srv/Cli | `/new_topic_name` | `sensor_msgs/Image` | Topic description |
+
+### ROS Parameter Changes
+
+#### Additions and removals
+
+| Change type   | Parameter Name | Type     | Default Value | Description       |
+|:--------------|:---------------|:---------|:--------------|:------------------|
+| Added/Removed | `param_name`   | `double` | `1.0`         | Param description |
+
+#### Modifications
+
+| Version | Parameter Name   | Type     | Default Value | Description       |
+|:--------|:-----------------|:---------|:--------------|:------------------|
+| Old     | `old_param_name` | `double` | `1.0`         | Param description |
+| New     | `new_param_name` | `double` | `1.0`         | Param description |
+
+🔴⬆️ -->
+
+## Effects on system behavior
+
+None.

.github/PULL_REQUEST_TEMPLATE/small-change.md

@@ -7,9 +7,6 @@
     - source: .github/ISSUE_TEMPLATE/bug.yaml
     - source: .github/ISSUE_TEMPLATE/config.yml
     - source: .github/ISSUE_TEMPLATE/task.yaml
-    - source: .github/PULL_REQUEST_TEMPLATE.md
-    - source: .github/PULL_REQUEST_TEMPLATE/small-change.md
-    - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md
     - source: .github/dependabot.yaml
     - source: .github/stale.yml
     - source: .github/workflows/cancel-previous-workflows.yaml

.github/PULL_REQUEST_TEMPLATE/standard-change.md

@@ -0,0 +1,25 @@
+name: Comment on PR
+on:
+  pull_request:
+
+jobs:
+  comment-on-pr:
+    runs-on: ubuntu-latest
+    permissions:
+      pull-requests: write
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+
+      - name: Initial PR comment
+        uses: marocchino/sticky-pull-request-comment@v2
+        with:
+          message: |
+            Thank you for contributing to the Autoware project!
+
+            🚧 If your pull request is in progress, [switch it to draft mode](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/changing-the-stage-of-a-pull-request#converting-a-pull-request-to-a-draft).
+
+            Please ensure:
+            - You've checked our [contribution guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/).
+            - Your PR follows our [pull request guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/).
+            - All required CI checks pass before [marking the PR ready for review](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/changing-the-stage-of-a-pull-request#marking-a-pull-request-as-ready-for-review).

.github/pull_request_template.md

@@ -297,7 +297,7 @@
 for curDir, dirs, files in os.walk(control_dir_path):
     for name in files:
         if name == "vehicle_cmd_gate.param.yaml":
-            if curDir.split("/")[-2] == "vehicle_cmd_gate":
+            if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate":
                 limit_yaml_path = curDir + "/" + name
                 break
 

.github/sync-files.yaml

@@ -16,6 +16,7 @@
 
 # cspell: ignore interp
 
+from enum import Enum
 import time
 
 from autoware_adapi_v1_msgs.msg import OperationModeState
@@ -26,6 +27,9 @@
 from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
 from autoware_vehicle_msgs.msg import SteeringReport
 from builtin_interfaces.msg import Duration
+from diagnostic_msgs.msg import DiagnosticStatus
+import diagnostic_updater
+from diagnostic_updater import DiagnosticStatusWrapper
 from geometry_msgs.msg import AccelWithCovarianceStamped
 from geometry_msgs.msg import PoseStamped
 from nav_msgs.msg import Odometry
@@ -42,6 +46,13 @@
 from tier4_debug_msgs.msg import Float32Stamped
 
 
+class ControlStatus(Enum):
+    DRIVE = 0
+    STOPPING = 1
+    STOPPED = 2
+    EMERGENCY = 3
+
+
 def getYaw(orientation_xyzw):
     return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2]
 
@@ -272,6 +283,10 @@ def __init__(self):
         self.last_steer_cmd = 0.0
         self.past_control_trajectory_mode = 1
 
+        self.diagnostic_updater = diagnostic_updater.Updater(self)
+        self.setup_diagnostic_updater()
+        self.control_state = ControlStatus.STOPPED
+
     def onTrajectory(self, msg):
         self._present_trajectory = msg
 
@@ -421,12 +436,14 @@ def control(self):
         orientation_deviation_threshold = 60 * np.pi / 180.0
 
         # The moment the threshold is exceeded, it enters emergency stop mode.
+        control_state = self.control_state
         if is_applying_control:
             if (
                 position_deviation > position_deviation_threshold
                 or orientation_deviation > orientation_deviation_threshold
             ):
                 self.emergency_stop_mode_flag = True
+                control_state = ControlStatus.EMERGENCY
 
             # Normal return from emergency stop mode when within the threshold value and a request to cancel the stop mode has been received.
             if (
@@ -698,12 +715,13 @@ def control(self):
                 steer_cmd = self.last_steer_cmd + steer_cmd_decrease_limit
             else:
                 steer_cmd = 0.0
+            control_state = ControlStatus.STOPPING
 
         cmd_msg = Control()
         cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = (
             self.get_clock().now().to_msg()
         )
-        cmd_msg.longitudinal.speed = trajectory_longitudinal_velocity[nearestIndex]
+        cmd_msg.longitudinal.velocity = trajectory_longitudinal_velocity[nearestIndex]
         cmd_msg.longitudinal.acceleration = acc_cmd
         cmd_msg.lateral.steering_tire_angle = steer_cmd
 
@@ -722,6 +740,15 @@ def control(self):
                 self.control_cmd_steer_list.pop(0)
                 self.control_cmd_acc_list.pop(0)
 
+        # [3-6] Update control state
+        if control_state != ControlStatus.EMERGENCY:
+            stopped_velocity_threshold = 0.1
+            if present_linear_velocity[0] <= stopped_velocity_threshold:
+                control_state = ControlStatus.STOPPED
+            elif control_state != ControlStatus.STOPPING:
+                control_state = ControlStatus.DRIVE
+        self.control_state = control_state
+
         # [4] Update MPC internal variables
         if not is_applying_control:
             self.controller.send_initialize_input_queue()
@@ -843,6 +870,23 @@ def control(self):
                 )
             )
 
+            self.diagnostic_updater.force_update()
+
+    def setup_diagnostic_updater(self):
+        self.diagnostic_updater.setHardwareID("pympc_trajectory_follower")
+        self.diagnostic_updater.add("control_state", self.check_control_state)
+
+    def check_control_state(self, stat: DiagnosticStatusWrapper):
+        msg = "emergency occurred" if self.control_state == ControlStatus.EMERGENCY else "OK"
+        level = (
+            DiagnosticStatus.ERROR
+            if self.control_state == ControlStatus.EMERGENCY
+            else DiagnosticStatus.OK
+        )
+        stat.summary(level, msg)
+        stat.add("control_state", str(self.control_state))
+        return stat
+
 
 def main(args=None):
     rclpy.init(args=args)

.github/workflows/comment_on_pr.yaml

@@ -28,7 +28,7 @@
     ]
 )
 setup(
-    name="smart_mpc_trajectory_follower",
+    name="autoware_smart_mpc_trajectory_follower",
     version="1.0.0",
     packages=find_packages(),
     package_data={

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py

@@ -444,7 +444,7 @@ def launch_setup(context, *args, **kwargs):
     smart_mpc_trajectory_follower = Node(
         package="autoware_smart_mpc_trajectory_follower",
         executable="pympc_trajectory_follower.py",
-        name="pympc_trajectory_follower",
+        name="controller_node_exe",
     )
     if trajectory_follower_mode == "trajectory_follower_node":
         return [group, control_validator_group]

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py

@@ -29,7 +29,7 @@ nav:
         - 'No Stopping Area': planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module
         - 'Occlusion Spot': planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module
         - 'Run Out': planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module
-        - 'Speed Bump': planning/behavior_velocity_planner/behavior_velocity_speed_bump_module
+        - 'Speed Bump': planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module
         - 'Stop Line': planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module
         - 'Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module
         - 'Virtual Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module