control/lane_departure_checker/README.md

@@ -43,6 +43,10 @@ This package includes the following features:
 
 2. Expand footprint based on the standard deviation multiplied with `footprint_margin_scale`.
 
+### How to extend footprint by constant
+
+Expand footprint based on the constant defined with `footprint_extra_margin`.
+
 ## Interface
 
 ### Input
@@ -72,6 +76,7 @@ This package includes the following features:
 | Name                       | Type   | Description                                                                        | Default value |
 | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ |
 | footprint_margin_scale     | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation.    | 1.0           |
+| footprint_extra_margin     | double | Coefficient for expanding footprint margin. When checking for lane departure.[m]   | 0.0           |
 | resample_interval          | double | Minimum Euclidean distance between points when resample trajectory.[m]             | 0.3           |
 | max_deceleration           | double | Maximum deceleration when calculating braking distance.                            | 2.8           |
 | delay_time                 | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3           |

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -6,6 +6,7 @@
 
     # Core
     footprint_margin_scale: 1.0
+    footprint_extra_margin: 0.0
     resample_interval: 0.3
     max_deceleration: 2.8
     delay_time: 1.3

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -50,6 +50,7 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 struct Param
 {
   double footprint_margin_scale;
+  double footprint_extra_margin;
   double resample_interval;
   double max_deceleration;
   double delay_time;

control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp

@@ -89,4 +89,9 @@ inline FootprintMargin calcFootprintMargin(
   return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale};
 }
 
+inline FootprintMargin operator+(FootprintMargin fm, const double margin)
+{
+  return FootprintMargin{fm.lon + margin, fm.lat + margin};
+}
+
 #endif  // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -219,7 +219,8 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   const Param & param)
 {
   // Calculate longitudinal and lateral margin based on covariance
-  const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale);
+  const auto margin =
+    calcFootprintMargin(covariance, param.footprint_margin_scale) + param.footprint_extra_margin;
 
   // Create vehicle footprint in base_link coordinate
   const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_, margin);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -128,6 +128,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   vehicle_length_m_ = vehicle_info.vehicle_length_m;
 
   param_.footprint_margin_scale = declare_parameter("footprint_margin_scale", 1.0);
+  param_.footprint_extra_margin = declare_parameter("footprint_extra_margin", 0.0);
   param_.resample_interval = declare_parameter("resample_interval", 0.3);
   param_.max_deceleration = declare_parameter("max_deceleration", 3.0);
   param_.delay_time = declare_parameter("delay_time", 0.3);
@@ -352,6 +353,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
 
     // Core
     update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale);
+    update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin);
     update_param(parameters, "resample_interval", param_.resample_interval);
     update_param(parameters, "max_deceleration", param_.max_deceleration);
     update_param(parameters, "delay_time", param_.delay_time);

system/dummy_infrastructure/config/dummy_infrastructure.param.yaml

@@ -1,3 +1,8 @@
 /**:
   ros__parameters:
     update_rate_hz: 10.0
+    use_first_command: false
+    use_command_state: true
+    instrument_id: '0'
+    approval: true
+    is_finalized: true

system/dummy_infrastructure/include/dummy_infrastructure/dummy_infrastructure_node.hpp

@@ -41,6 +41,7 @@ class DummyInfrastructureNode : public rclcpp::Node
   {
     double update_rate_hz{};
     bool use_first_command{};
+    bool use_command_state{};
     std::string instrument_id{};
     bool approval{};
     bool is_finalized{};

system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp

@@ -47,21 +47,37 @@ bool update_param(
   return true;
 }
 
-boost::optional<InfrastructureCommand> findCommand(
+boost::optional<InfrastructureCommandArray> findCommand(
   const InfrastructureCommandArray & command_array, const std::string & instrument_id,
-  const bool use_first_command)
+  const bool use_first_command, const bool use_command_state)
 {
+  InfrastructureCommandArray array;
+  bool found_flag = false;
   if (use_first_command && !command_array.commands.empty()) {
-    return command_array.commands.front();
+    array.commands.push_back(command_array.commands.front());
+    return array;
+  }
+
+  if (use_command_state) {
+    for (const auto & command : command_array.commands) {
+      if (command.state >= InfrastructureCommand::REQUESTING) {
+        array.commands.push_back(command);
+        found_flag = true;
+      }
+    }
   }
 
   for (const auto & command : command_array.commands) {
     if (command.id == instrument_id) {
-      return command;
+      array.commands.push_back(command);
+      found_flag = true;
     }
   }
-
-  return {};
+  if (found_flag) {
+    return array;
+  } else {
+    return {};
+  }
 }
 }  // namespace
 
@@ -75,6 +91,7 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod
   // Parameter
   node_param_.update_rate_hz = declare_parameter<double>("update_rate_hz", 10.0);
   node_param_.use_first_command = declare_parameter<bool>("use_first_command", true);
+  node_param_.use_command_state = declare_parameter<bool>("use_command_state", false);
   node_param_.instrument_id = declare_parameter<std::string>("instrument_id", "");
   node_param_.approval = declare_parameter<bool>("approval", false);
   node_param_.is_finalized = declare_parameter<bool>("is_finalized", false);
@@ -95,7 +112,9 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod
 
 void DummyInfrastructureNode::onCommandArray(const InfrastructureCommandArray::ConstSharedPtr msg)
 {
-  command_array_ = msg;
+  if (!msg->commands.empty()) {
+    command_array_ = msg;
+  }
 }
 
 rcl_interfaces::msg::SetParametersResult DummyInfrastructureNode::onSetParam(
@@ -110,6 +129,7 @@ rcl_interfaces::msg::SetParametersResult DummyInfrastructureNode::onSetParam(
     // Update params
     update_param(params, "update_rate_hz", p.update_rate_hz);
     update_param(params, "use_first_command", p.use_first_command);
+    update_param(params, "use_command_state", p.use_command_state);
     update_param(params, "instrument_id", p.instrument_id);
     update_param(params, "approval", p.approval);
     update_param(params, "is_finalized", p.is_finalized);
@@ -142,20 +162,33 @@ void DummyInfrastructureNode::onTimer()
   if (!isDataReady()) {
     return;
   }
-
-  const auto command =
-    findCommand(*command_array_, node_param_.instrument_id, node_param_.use_first_command);
-
-  VirtualTrafficLightState state;
-  state.stamp = get_clock()->now();
-  state.id = command ? command->id : node_param_.instrument_id;
-  state.type = "dummy_infrastructure";
-  state.approval = command ? node_param_.approval : false;
-  state.is_finalized = node_param_.is_finalized;
-
   VirtualTrafficLightStateArray state_array;
   state_array.stamp = get_clock()->now();
-  state_array.states.push_back(state);
+
+  const auto found_command_array = findCommand(
+    *command_array_, node_param_.instrument_id, node_param_.use_first_command,
+    node_param_.use_command_state);
+
+  if (!found_command_array) {
+    VirtualTrafficLightState state;
+    state.stamp = get_clock()->now();
+    state.id = node_param_.instrument_id;
+    state.type = "dummy_infrastructure";
+    state.approval = false;
+    state.is_finalized = node_param_.is_finalized;
+    state_array.states.push_back(state);
+  } else {
+    for (auto command : found_command_array->commands) {
+      VirtualTrafficLightState state;
+      state.stamp = get_clock()->now();
+      state.id = command.id;
+      state.type = command.type;
+      state.approval = node_param_.approval;
+      state.is_finalized = node_param_.is_finalized;
+      state_array.states.push_back(state);
+    }
+  }
+
   pub_state_array_->publish(state_array);
 }
 

system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml

@@ -29,3 +29,15 @@
               path: localization_accuracy
               contains: [": localization_accuracy"]
               timeout: 1.0
+
+            potential_lane_departure:
+              type: diagnostic_aggregator/GenericAnalyzer
+              path: potential_lane_departure
+              contains: [": potential_lane_departure"]
+              timeout: 0.3
+
+            potential_stopline_overrun:
+              type: diagnostic_aggregator/GenericAnalyzer
+              path: potential_stopline_overrun
+              contains: [": potential_stopline_overrun"]
+              timeout: 0.3