diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index fb96e134f7746..bd6cf28c0b629 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -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 diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 78a120fe601f2..196657fab6e91 100755 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -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) diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py index fee1d04e826c0..7df489ff29377 100644 --- a/control/autoware_smart_mpc_trajectory_follower/setup.py +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -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={ diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 1cf3cb5b44656..15cab8df52ede 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.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]