Merge branch 'main' into feat/add_pose_instability_detector

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -497,8 +497,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
   const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;
 
-  const bool keep_stopped_condition =
-    m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
+  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
+  static constexpr double vel_epsilon = 1e-3;
+
+  // Let vehicle start after the steering is converged for control accuracy
+  const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
+                                      m_enable_keep_stopped_until_steer_convergence &&
+                                      !lateral_sync_data_.is_steer_converged;
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
@@ -528,8 +533,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
-  static constexpr double vel_epsilon =
-    1e-3;  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
   const double current_vel_cmd =
     std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&

control/vehicle_cmd_gate/README.md

@@ -50,6 +50,8 @@
 | `check_external_emergency_heartbeat`        | bool     | true when checking heartbeat for emergency stop                                                                                                                                             |
 | `system_emergency_heartbeat_timeout`        | double   | timeout for system emergency                                                                                                                                                                |
 | `external_emergency_stop_heartbeat_timeout` | double   | timeout for external emergency                                                                                                                                                              |
+| `filter_activated_count_threshold`          | int      | threshold for filter activation                                                                                                                                                             |
+| `filter_activated_velocity_threshold`       | double   | velocity threshold for filter activation                                                                                                                                                    |
 | `stop_hold_acceleration`                    | double   | longitudinal acceleration cmd when vehicle should stop                                                                                                                                      |
 | `emergency_acceleration`                    | double   | longitudinal acceleration cmd when vehicle stop with emergency                                                                                                                              |
 | `moderate_stop_service_acceleration`        | double   | longitudinal acceleration cmd when vehicle stop with moderate stop service                                                                                                                  |

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -6,6 +6,8 @@
     check_external_emergency_heartbeat: false
     use_start_request: false
     enable_cmd_limit_filter: true
+    filter_activated_count_threshold: 5
+    filter_activated_velocity_threshold: 1.0
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -77,6 +77,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     create_publisher<IsFilterActivated>("~/is_filter_activated", durable_qos);
   filter_activated_marker_pub_ =
     create_publisher<MarkerArray>("~/is_filter_activated/marker", durable_qos);
+  filter_activated_marker_raw_pub_ =
+    create_publisher<MarkerArray>("~/is_filter_activated/marker_raw", durable_qos);
+  filter_activated_flag_pub_ =
+    create_publisher<BoolStamped>("~/is_filter_activated/flag", durable_qos);
 
   // Subscriber
   external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
@@ -160,6 +164,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     declare_parameter<double>("moderate_stop_service_acceleration");
   stop_check_duration_ = declare_parameter<double>("stop_check_duration");
   enable_cmd_limit_filter_ = declare_parameter<bool>("enable_cmd_limit_filter");
+  filter_activated_count_threshold_ = declare_parameter<int>("filter_activated_count_threshold");
+  filter_activated_velocity_threshold_ =
+    declare_parameter<double>("filter_activated_velocity_threshold");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -572,7 +579,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
 
   is_filter_activated.stamp = now();
   is_filter_activated_pub_->publish(is_filter_activated);
-  filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated));
+  publishMarkers(is_filter_activated);
 
   return out;
 }
@@ -787,6 +794,28 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a
   return msg;
 }
 
+void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated)
+{
+  BoolStamped filter_activated_flag;
+  if (filter_activated.is_activated) {
+    filter_activated_count_++;
+  } else {
+    filter_activated_count_ = 0;
+  }
+  if (
+    filter_activated_count_ >= filter_activated_count_threshold_ &&
+    std::fabs(current_kinematics_.twist.twist.linear.x) >= filter_activated_velocity_threshold_ &&
+    current_operation_mode_.mode == OperationModeState::AUTONOMOUS) {
+    filter_activated_marker_pub_->publish(createMarkerArray(filter_activated));
+    filter_activated_flag.data = true;
+  } else {
+    filter_activated_flag.data = false;
+  }
+
+  filter_activated_flag.stamp = now();
+  filter_activated_flag_pub_->publish(filter_activated_flag);
+  filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated));
+}
 }  // namespace vehicle_cmd_gate
 
 #include <rclcpp_components/register_node_macro.hpp>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -62,6 +62,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using std_srvs::srv::Trigger;
 using tier4_control_msgs::msg::GateMode;
+using tier4_debug_msgs::msg::BoolStamped;
 using tier4_external_api_msgs::msg::Emergency;
 using tier4_external_api_msgs::msg::Heartbeat;
 using tier4_external_api_msgs::srv::SetEmergency;
@@ -106,6 +107,8 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_pub_;
   rclcpp::Publisher<IsFilterActivated>::SharedPtr is_filter_activated_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_raw_pub_;
+  rclcpp::Publisher<BoolStamped>::SharedPtr filter_activated_flag_pub_;
 
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
@@ -123,11 +126,13 @@ class VehicleCmdGate : public rclcpp::Node
   bool is_engaged_;
   bool is_system_emergency_ = false;
   bool is_external_emergency_stop_ = false;
+  bool is_filtered_marker_published_ = false;
   double current_steer_ = 0;
   GateMode current_gate_mode_;
   MrmState current_mrm_state_;
   Odometry current_kinematics_;
   double current_acceleration_ = 0.0;
+  int filter_activated_count_ = 0;
 
   // Heartbeat
   std::shared_ptr<rclcpp::Time> emergency_state_heartbeat_received_time_;
@@ -172,6 +177,8 @@ class VehicleCmdGate : public rclcpp::Node
   double emergency_acceleration_;
   double moderate_stop_service_acceleration_;
   bool enable_cmd_limit_filter_;
+  int filter_activated_count_threshold_;
+  double filter_activated_velocity_threshold_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;
@@ -236,6 +243,7 @@ class VehicleCmdGate : public rclcpp::Node
 
   // debug
   MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
+  void publishMarkers(const IsFilterActivated & filter_activated);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 };

system/system_diagnostic_graph/CMakeLists.txt

@@ -4,23 +4,27 @@ project(system_diagnostic_graph)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(aggregator
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/core/config.cpp
   src/core/debug.cpp
   src/core/graph.cpp
-  src/core/nodes.cpp
-  src/core/exprs.cpp
+  src/core/units.cpp
   src/core/modes.cpp
+)
+
+ament_auto_add_executable(aggregator
   src/main.cpp
 )
 
 ament_auto_add_executable(converter
   src/tool.cpp
 )
 
-install(
-  PROGRAMS util/debug.py
-  DESTINATION lib/${PROJECT_NAME}
-)
+if(BUILD_TESTING)
+  get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE)
+  ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp)
+  target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}")
+  target_include_directories(gtest_${PROJECT_NAME} PRIVATE src)
+endif()
 
 ament_auto_package(INSTALL_TO_SHARE config example launch)

system/system_diagnostic_graph/example/example_0.yaml

@@ -1,32 +1,37 @@
 files:
-  - { package: system_diagnostic_graph, path: example/example_1.yaml }
-  - { package: system_diagnostic_graph, path: example/example_2.yaml }
+  - { path: $(find-pkg-share system_diagnostic_graph)/example/example_1.yaml }
+  - { path: $(find-pkg-share system_diagnostic_graph)/example/example_2.yaml }
 
 nodes:
   - path: /autoware/modes/stop
-    type: debug-ok
+    type: ok
 
   - path: /autoware/modes/autonomous
     type: and
     list:
-      - { type: link, path: /functions/pose_estimation }
-      - { type: link, path: /functions/obstacle_detection }
+      - { type: link, link: /functions/pose_estimation }
+      - { type: link, link: /functions/obstacle_detection }
 
   - path: /autoware/modes/local
     type: and
     list:
-      - { type: link, path: /external/joystick_command }
+      - { type: link, link: /external/joystick_command }
 
   - path: /autoware/modes/remote
     type: and
     list:
-      - { type: link, path: /external/remote_command }
+      - { type: link, link: /external/remote_command }
 
   - path: /autoware/modes/emergency-stop
-    type: debug-ok
+    type: ok
 
   - path: /autoware/modes/comfortable-stop
-    type: debug-ok
+    type: and
+    list:
+      - { type: link, link: /functions/obstacle_detection }
 
   - path: /autoware/modes/pull-over
-    type: debug-ok
+    type: and
+    list:
+      - { type: link, link: /functions/pose_estimation }
+      - { type: link, link: /functions/obstacle_detection }

system/system_diagnostic_graph/example/example_1.yaml

@@ -2,22 +2,22 @@ nodes:
   - path: /functions/pose_estimation
     type: and
     list:
-      - { type: link, path: /sensing/lidars/top }
+      - { type: link, link: /sensing/lidars/top }
 
   - path: /functions/obstacle_detection
     type: or
     list:
-      - { type: link, path: /sensing/lidars/front }
-      - { type: link, path: /sensing/radars/front }
+      - { type: link, link: /sensing/lidars/front }
+      - { type: link, link: /sensing/radars/front }
 
   - path: /sensing/lidars/top
     type: diag
-    name: "lidar_driver/top: status"
+    diag: "lidar_driver/top: status"
 
   - path: /sensing/lidars/front
     type: diag
-    name: "lidar_driver/front: status"
+    diag: "lidar_driver/front: status"
 
   - path: /sensing/radars/front
     type: diag
-    name: "radar_driver/front: status"
+    diag: "radar_driver/front: status"

system/system_diagnostic_graph/example/example_2.yaml

@@ -1,8 +1,8 @@
 nodes:
   - path: /external/joystick_command
     type: diag
-    name: "external_command_checker: joystick_command"
+    diag: "external_command_checker: joystick_command"
 
   - path: /external/remote_command
     type: diag
-    name: "external_command_checker: remote_command"
+    diag: "external_command_checker: remote_command"

system/system_diagnostic_graph/package.xml

@@ -15,6 +15,7 @@
   <depend>tier4_system_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>