fix(smart_mpc_trajectory_folower): fix running by adding control_state and changing msg/package_name

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
 

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py

@@ -1,4 +1,4 @@
 #!/usr/bin/env python3
 
 # Copyright 2024 Proxima Technology Inc, TIER IV
 #
@@ -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 @@
         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,428 +436,457 @@
         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 (
                 not (
                     position_deviation > position_deviation_threshold
                     or orientation_deviation > orientation_deviation_threshold
                 )
                 and self.stop_mode_reset_request
             ):
                 self.emergency_stop_mode_flag = False
 
         # Processes to ensure that requests do not continue to remain after the stop mode is cancelled.
         if not self.emergency_stop_mode_flag:
             self.stop_mode_reset_request = False
 
         # [3] Control logic calculation
         # [3-1] pure pursuit
         # [3-1-1] compute pure pursuit acc cmd
         longitudinal_vel_err = (
             present_linear_velocity - trajectory_longitudinal_velocity[nearestIndex]
         )
         acc_kp = 0.5
         acc_lim = 2.0
         pure_pursuit_acc_cmd = np.clip(-acc_kp * longitudinal_vel_err, -acc_lim, acc_lim)[0]
 
         # [3-1-2] compute pure pursuit steer cmd
         nearest_trajectory_point_yaw = getYaw(trajectory_orientation[nearestIndex])
         cos_yaw = np.cos(nearest_trajectory_point_yaw)
         sin_yaw = np.sin(nearest_trajectory_point_yaw)
         diff_position = present_position - trajectory_position[nearestIndex]
         lat_err = -sin_yaw * diff_position[0] + cos_yaw * diff_position[1]
         yaw_err = getYaw(present_orientation) - nearest_trajectory_point_yaw
         while True:
             if yaw_err > np.pi:
                 yaw_err -= 2.0 * np.pi
             if yaw_err < (-np.pi):
                 yaw_err += 2.0 * np.pi
             if np.abs(yaw_err) < np.pi:
                 break
         wheel_base = drive_functions.L  # 4.0
         lookahead_time = 3.0
         min_lookahead = 3.0
         lookahead = min_lookahead + lookahead_time * np.abs(present_linear_velocity[0])
         steer_kp = 2.0 * wheel_base / (lookahead * lookahead)
         steer_kd = 2.0 * wheel_base / lookahead
         steer_lim = 0.6
         pure_pursuit_steer_cmd = np.clip(
             -steer_kp * lat_err - steer_kd * yaw_err, -steer_lim, steer_lim
         )[0]
 
         # [3-2] MPC control logic calculation
         # [3-2-1] State variables in the MPC
         present_mpc_x = np.array(
             [
                 present_position[0],
                 present_position[1],
                 present_linear_velocity[0],
                 getYaw(present_orientation)[0],
                 present_linear_acceleration[0],
                 present_steering_tire_angle,
             ]
         )
 
         # [3-2-2] resampling trajectory by time (except steer angle).
         X_des = np.zeros((drive_functions.N + 1, 8))
         dist = np.sqrt(((trajectory_position[1:] - trajectory_position[:-1]) ** 2).sum(axis=1))
         timestamp_from_start = [0.0]
         tmp_t = 0.0
         for i in range(len(dist)):
             tmp_t += dist[i] / max(np.abs(trajectory_longitudinal_velocity[i]), 0.1)
             timestamp_from_start.append(1 * tmp_t)
         timestamp_from_start = np.array(timestamp_from_start)
 
         mpc_trajectory_time_step = 0.1
         timestamp_mpc = (
             np.arange(drive_functions.N + 1) * mpc_trajectory_time_step
             + timestamp_from_start[nearestIndex]
         )
         for i in range(drive_functions.N + 1):
             if timestamp_mpc[drive_functions.N - i] > timestamp_from_start[-1]:
                 timestamp_mpc[drive_functions.N - i] = timestamp_from_start[-1]
             else:
                 break
         pos_x_interpolate = scipy.interpolate.interp1d(
             timestamp_from_start, trajectory_position[:, 0]
         )
         pos_y_interpolate = scipy.interpolate.interp1d(
             timestamp_from_start, trajectory_position[:, 1]
         )
         longitudinal_velocity_interpolate = scipy.interpolate.interp1d(
             timestamp_from_start, trajectory_longitudinal_velocity
         )
         key_rots = R.from_quat(trajectory_orientation.reshape(-1, 4))
         orientation_interpolate = Slerp(timestamp_from_start, key_rots)
         acceleration_interpolate = scipy.interpolate.interp1d(
             timestamp_from_start, trajectory_acceleration
         )
 
         X_des[:, 0] = pos_x_interpolate(timestamp_mpc)
         X_des[:, 1] = pos_y_interpolate(timestamp_mpc)
         X_des[:, 2] = longitudinal_velocity_interpolate(timestamp_mpc)
         X_des[:, 3] = orientation_interpolate(timestamp_mpc).as_euler("xyz")[:, 2]
         X_des[:, 4] = acceleration_interpolate(timestamp_mpc)
         X_des[:, 6] = 1 * X_des[:, 4]
 
         # [3-2-3] resampling trajectory by time (steer angle)
         previous_des_steer = 0.0
         steer_trajectory2 = np.zeros(len(timestamp_mpc))
         downsampling = 5
         for ii in range(2 + (int(len(timestamp_mpc) // downsampling))):
             i = ii * downsampling
             if i >= len(timestamp_mpc):
                 i = len(timestamp_mpc) - 1
                 if (i % downsampling) == 0:
                     continue
 
             tmp_des_pos = 1 * X_des[i, :2]
             distance_squared = ((trajectory_position[:, :2] - tmp_des_pos[:2]) ** 2).sum(axis=1)
             tmp_nearest_index = distance_squared.argmin()
             curvature_calculation_distance = 3.0
             nearestIndex_in_front = (
                 np.abs(
                     distance_squared[tmp_nearest_index:]
                     - curvature_calculation_distance * curvature_calculation_distance
                 )
             ).argmin() + tmp_nearest_index
             distance_in_front = np.sqrt(distance_squared[nearestIndex_in_front])
             if distance_in_front < curvature_calculation_distance:
                 curvature_calculation_distance = distance_in_front
             if tmp_nearest_index > 0:
                 nearestIndex_behind = (
                     np.abs(
                         distance_squared[:tmp_nearest_index]
                         - curvature_calculation_distance * curvature_calculation_distance
                     )
                 ).argmin()
                 curvature_calculation_distance_threshold = 0.5
                 if distance_in_front < curvature_calculation_distance_threshold:
                     # If there is no point in front, fill in with the value before it.
                     tmp_des_steer = 1 * previous_des_steer
                 else:
                     # Normal line passing through a point 3 m behind.：b1*x + b2*y + b3 = 0
                     b1 = tmp_des_pos[0] - trajectory_position[nearestIndex_behind][0]
                     b2 = tmp_des_pos[1] - trajectory_position[nearestIndex_behind][1]
                     b3 = (
                         -b1 * trajectory_position[nearestIndex_behind][0]
                         - b2 * trajectory_position[nearestIndex_behind][1]
                     )
                     # Normal line through a point 3 m in front：f1*x + f2*y + f3 = 0
                     f1 = tmp_des_pos[0] - trajectory_position[nearestIndex_in_front][0]
                     f2 = tmp_des_pos[1] - trajectory_position[nearestIndex_in_front][1]
                     f3 = (
                         -f1 * trajectory_position[nearestIndex_in_front][0]
                         - f2 * trajectory_position[nearestIndex_in_front][1]
                     )
                     det = b1 * f2 - b2 * f1
                     if np.abs(det) < 1e-6:
                         # The two normals have the same slope, so steer is 0
                         tmp_des_steer = 0.0
                     else:
                         # solve Ax+b=0
                         invA = np.array([[f2, -b2], [-f1, b1]]) / det
                         sol = -invA @ np.array([b3, f3])  # center of the circle
                         curvature = np.sqrt(
                             ((sol - trajectory_position[nearestIndex_behind][:2]) ** 2).sum()
                         )
                         tmp_des_steer = np.arctan(drive_functions.L / curvature)
 
                         tmp_vec_behind = sol - trajectory_position[nearestIndex_behind][:2]
                         # Line segment connecting backward point - centre of circle
                         cross_product = b1 * tmp_vec_behind[1] - b2 * tmp_vec_behind[0]
                         if cross_product < 0.0:
                             tmp_des_steer = -tmp_des_steer
 
                 steer_trajectory2[i] = 1.0 * tmp_des_steer
                 previous_des_steer = 1.0 * tmp_des_steer
             else:
                 steer_trajectory2[i] = 0.0
                 previous_des_steer = 0.0
 
         for i in range(len(timestamp_mpc) - 1):
             reminder = i % downsampling
             i0 = i - reminder
             i1 = i0 + downsampling
             if reminder == 0:
                 continue
             if i1 >= len(timestamp_mpc):
                 i1 = len(timestamp_mpc) - 1
             w = (1.0 * reminder) / (i1 - i0)
             steer_trajectory2[i] = (1 - w) * steer_trajectory2[i0] + w * steer_trajectory2[i1]
 
         X_des[:, 5] = steer_trajectory2
         X_des[:, 7] = 1 * X_des[:, 5]
 
         # [3-2-4] mpc computation
         U_des = np.zeros((drive_functions.N, 2))
         start_calc_u_opt = time.time()
         u_opt = self.controller.update_input_queue_and_get_optimal_control(
             self.control_cmd_time_stamp_list,
             self.control_cmd_acc_list,
             self.control_cmd_steer_list,
             present_mpc_x,
             X_des,
             U_des,
         )
         end_calc_u_opt = time.time()
         mpc_acc_cmd = u_opt[0]
         mpc_steer_cmd = u_opt[1]
 
         # [3-3] Enter goal stop mode (override command value) to maintain stop at goal
         self.goal_stop_mode_flag = False
         if self._goal_pose is not None:
             goal_position = np.array(
                 [
                     self._goal_pose.pose.position.x,
                     self._goal_pose.pose.position.y,
                     self._goal_pose.pose.position.z,
                 ]
             )
 
             distance_from_goal = np.sqrt(((goal_position[:2] - present_position[:2]) ** 2).sum())
             goal_distance_threshold = 0.8
             if distance_from_goal < goal_distance_threshold:
                 self.goal_stop_mode_flag = True
 
         # [3-4] Override when doing step response
         if self.step_response_trigger:
             self.step_response_time_step = 0
             self.step_response_flag = True
             self.step_response_trigger = False
 
         if self.step_response_flag:
             step_response_elapsed_time = self.step_response_time_step * self.timer_period_callback
             mpc_acc_cmd = 1.0 * self.last_acc_cmd
             if step_response_elapsed_time < 3.0:
                 mpc_steer_cmd = 0.0
             else:
                 mpc_steer_cmd = -0.005
             self.get_logger().info(
                 "step_response_elapsed_time: "
                 + str(step_response_elapsed_time)
                 + ", mpc_steer_cmd: "
                 + str(mpc_steer_cmd)
             )
             self.step_response_time_step += 1
 
         # [3-5] Determine the control logic to be finally applied and publish it
         acc_cmd = 0.0
         steer_cmd = 0.0
         if (not self.emergency_stop_mode_flag) and (not self.goal_stop_mode_flag):
             # in normal mode
             acc_cmd = mpc_acc_cmd
             steer_cmd = mpc_steer_cmd
 
             overwrite_cmd_by_pure_pursuit_flag = False
             if overwrite_cmd_by_pure_pursuit_flag:
                 steer_cmd = pure_pursuit_steer_cmd
                 acc_cmd = pure_pursuit_acc_cmd
         else:
             # in stop mode
             acc_cmd_decrease_limit = 5.0 * self.timer_period_callback  # lon_jerk_lim
             steer_cmd_decrease_limit = 0.5 * self.timer_period_callback  # steer_rate_lim
             acc_cmd = max(
                 self.last_acc_cmd - acc_cmd_decrease_limit, -drive_functions.acc_lim_points[0]
             )  # lon_acc_lim
             if self.last_steer_cmd > (steer_cmd_decrease_limit + 1e-6):
                 steer_cmd = self.last_steer_cmd - steer_cmd_decrease_limit
             elif self.last_steer_cmd < -(steer_cmd_decrease_limit + 1e-6):
                 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
 
         self.control_cmd_pub_.publish(cmd_msg)
         self.last_acc_cmd = 1 * acc_cmd
         self.last_steer_cmd = 1 * steer_cmd
 
         if self.past_control_trajectory_mode == 1:
             self.control_cmd_time_stamp_list.append(
                 cmd_msg.stamp.sec + 1e-9 * cmd_msg.stamp.nanosec
             )
             self.control_cmd_steer_list.append(steer_cmd)
             self.control_cmd_acc_list.append(acc_cmd)
             if self.control_cmd_time_stamp_list[-1] - self.control_cmd_time_stamp_list[0] > 3.0:
                 self.control_cmd_time_stamp_list.pop(0)
                 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()
             self.controller.stop_model_update()
             self.control_cmd_time_stamp_list.clear()
             self.control_cmd_steer_list.clear()
             self.control_cmd_acc_list.clear()
 
         # [5] MPC prediction trajectory publish
         traj_msg = Trajectory()
         traj_msg.header.stamp = cmd_msg.stamp
         traj_msg.header.frame_id = "map"
         downsampling = 3
         for ii in range(int(self.controller.nominal_traj.shape[0] // downsampling)):
             i = ii * downsampling
             traj_msg.points += [TrajectoryPoint()]
             int_sec = int(i * mpc_trajectory_time_step)
             traj_msg.points[ii].time_from_start = Duration()
             traj_msg.points[ii].time_from_start.sec = int_sec
             traj_msg.points[ii].time_from_start.nanosec = int(
                 (i * mpc_trajectory_time_step - int_sec) * 1e9
             )
             traj_msg.points[ii].pose.position.x = self.controller.nominal_traj[i, 0]
             traj_msg.points[ii].pose.position.y = self.controller.nominal_traj[i, 1]
             traj_msg.points[ii].pose.position.z = present_position[2]
             tmp_orientation_xyzw = R.from_euler("z", self.controller.nominal_traj[i, 3]).as_quat()
             traj_msg.points[ii].pose.orientation.x = tmp_orientation_xyzw[0]
             traj_msg.points[ii].pose.orientation.y = tmp_orientation_xyzw[1]
             traj_msg.points[ii].pose.orientation.z = tmp_orientation_xyzw[2]
             traj_msg.points[ii].pose.orientation.w = tmp_orientation_xyzw[3]
             traj_msg.points[ii].longitudinal_velocity_mps = self.controller.nominal_traj[i, 2]
             traj_msg.points[ii].acceleration_mps2 = self.controller.nominal_traj[i, 4]
 
         self.predicted_trajectory_pub_.publish(traj_msg)
 
         # [-1] Publish internal information for debugging
         enable_debug_pub = True
         if enable_debug_pub:
             self.debug_mpc_emergency_stop_mode_pub_.publish(
                 BoolStamped(stamp=cmd_msg.stamp, data=self.emergency_stop_mode_flag)
             )
             self.debug_mpc_goal_stop_mode_pub_.publish(
                 BoolStamped(stamp=cmd_msg.stamp, data=self.goal_stop_mode_flag)
             )
             self.debug_mpc_x_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 0])
             )
             self.debug_mpc_y_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 1])
             )
             self.debug_mpc_v_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 2])
             )
             self.debug_mpc_yaw_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 3])
             )
             self.debug_mpc_acc_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 4])
             )
             self.debug_mpc_steer_des_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=X_des[:, 5])
             )
             self.debug_mpc_x_current_pub_.publish(
                 Float32MultiArrayStamped(stamp=cmd_msg.stamp, data=present_mpc_x.tolist())
             )
             self.debug_mpc_X_des_converted_pub_.publish(
                 Float32MultiArrayStamped(
                     stamp=cmd_msg.stamp, data=self.controller.X_des.flatten().tolist()
                 )
             )
             self.debug_mpc_nominal_traj_pub_.publish(
                 Float32MultiArrayStamped(
                     stamp=cmd_msg.stamp, data=self.controller.nominal_traj.flatten().tolist()
                 )
             )
             self.debug_mpc_nominal_inputs_pub_.publish(
                 Float32MultiArrayStamped(
                     stamp=cmd_msg.stamp, data=self.controller.nominal_inputs.flatten().tolist()
                 )
             )
             self.debug_mpc_X_input_list_pub_.publish(
                 Float32MultiArrayStamped(
                     stamp=cmd_msg.stamp,
                     data=np.array(self.controller.X_input_list[-1:]).flatten().tolist(),
                 )
             )
             self.debug_mpc_Y_output_list_pub_.publish(
                 Float32MultiArrayStamped(
                     stamp=cmd_msg.stamp,
                     data=np.array(self.controller.Y_output_list[-1:]).flatten().tolist(),
                 )
             )
             self.debug_mpc_max_trajectory_err_pub_.publish(
                 Float32Stamped(
                     stamp=cmd_msg.stamp,
                     data=self.controller.err,
                 )
             )
             if self.controller.use_trained_model:
                 self.debug_mpc_error_prediction_pub_.publish(
                     Float32MultiArrayStamped(
                         stamp=cmd_msg.stamp, data=self.controller.previous_error[:6]
                     )
                 )
 
             end_ctrl_time = time.time()
 
             self.debug_mpc_calc_u_opt_time_pub_.publish(
                 Float32Stamped(
                     stamp=cmd_msg.stamp,
                     data=(end_calc_u_opt - start_calc_u_opt),
                 )
             )
 
             self.debug_mpc_total_ctrl_time_pub_.publish(
                 Float32Stamped(
                     stamp=cmd_msg.stamp,
                     data=(end_ctrl_time - start_ctrl_time),
                 )
             )
 
+            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)

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={

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]