refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

control/autoware_lane_departure_checker/CMakeLists.txt

@@ -14,6 +14,7 @@ ament_auto_add_library(autoware_lane_departure_checker SHARED
   src/lane_departure_checker_node/lane_departure_checker.cpp
   src/lane_departure_checker_node/lane_departure_checker_node.cpp
   src/lane_departure_checker_node/utils.cpp
+  src/lane_departure_checker_node/parameters.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp

@@ -15,15 +15,12 @@
 #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_
 #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_
 
-#include <autoware/universe_utils/geometry/boost_geometry.hpp>
-#include <autoware/universe_utils/geometry/pose_deviation.hpp>
+#include "autoware/lane_departure_checker/parameters.hpp"
+
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 
-#include <autoware_planning_msgs/msg/lanelet_route.hpp>
-#include <autoware_planning_msgs/msg/trajectory.hpp>
-#include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
@@ -48,81 +45,33 @@
 
 namespace autoware::lane_departure_checker
 {
-using autoware::universe_utils::LinearRing2d;
-using autoware::universe_utils::PoseDeviation;
 using autoware::universe_utils::Segment2d;
-using autoware_planning_msgs::msg::LaneletRoute;
-using autoware_planning_msgs::msg::Trajectory;
-using autoware_planning_msgs::msg::TrajectoryPoint;
 using tier4_planning_msgs::msg::PathWithLaneId;
-using TrajectoryPoints = std::vector<TrajectoryPoint>;
 typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<16>> SegmentRtree;
 
-struct Param
-{
-  double footprint_margin_scale{0.0};
-  double footprint_extra_margin{0.0};
-  double resample_interval{0.0};
-  double max_deceleration{0.0};
-  double delay_time{0.0};
-  double max_lateral_deviation{0.0};
-  double max_longitudinal_deviation{0.0};
-  double max_yaw_deviation_deg{0.0};
-  double min_braking_distance{0.0};
-  // nearest search to ego
-  double ego_nearest_dist_threshold{0.0};
-  double ego_nearest_yaw_threshold{0.0};
-};
-
-struct Input
-{
-  nav_msgs::msg::Odometry::ConstSharedPtr current_odom{};
-  lanelet::LaneletMapPtr lanelet_map{};
-  LaneletRoute::ConstSharedPtr route{};
-  lanelet::ConstLanelets route_lanelets{};
-  lanelet::ConstLanelets shoulder_lanelets{};
-  Trajectory::ConstSharedPtr reference_trajectory{};
-  Trajectory::ConstSharedPtr predicted_trajectory{};
-  std::vector<std::string> boundary_types_to_detect{};
-};
-
-struct Output
-{
-  std::map<std::string, double> processing_time_map{};
-  bool will_leave_lane{};
-  bool is_out_of_lane{};
-  bool will_cross_boundary{};
-  PoseDeviation trajectory_deviation{};
-  lanelet::ConstLanelets candidate_lanelets{};
-  TrajectoryPoints resampled_trajectory{};
-  std::vector<LinearRing2d> vehicle_footprints{};
-  std::vector<LinearRing2d> vehicle_passing_areas{};
-};
-
 class LaneDepartureChecker
 {
 public:
-  LaneDepartureChecker(
+  explicit LaneDepartureChecker(
     std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
       std::make_shared<universe_utils::TimeKeeper>())
   : time_keeper_(time_keeper)
   {
   }
-  Output update(const Input & input);
 
-  void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info)
+  LaneDepartureChecker(
+    const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+    std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
+      std::make_shared<universe_utils::TimeKeeper>())
+  : param_(param),
+    vehicle_info_ptr_(std::make_shared<autoware::vehicle_info_utils::VehicleInfo>(vehicle_info)),
+    time_keeper_(time_keeper)
   {
-    param_ = param;
-    vehicle_info_ptr_ = std::make_shared<autoware::vehicle_info_utils::VehicleInfo>(vehicle_info);
   }
+  Output update(const Input & input);
 
   void setParam(const Param & param) { param_ = param; }
 
-  void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info)
-  {
-    vehicle_info_ptr_ = std::make_shared<autoware::vehicle_info_utils::VehicleInfo>(vehicle_info);
-  }
-
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_
 
 #include "autoware/lane_departure_checker/lane_departure_checker.hpp"
+#include "autoware/lane_departure_checker/parameters.hpp"
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
@@ -47,21 +48,6 @@ namespace autoware::lane_departure_checker
 {
 using autoware_map_msgs::msg::LaneletMapBin;
 
-struct NodeParam
-{
-  bool will_out_of_lane_checker;
-  bool out_of_lane_checker;
-  bool boundary_departure_checker;
-
-  double update_rate;
-  bool visualize_lanelet;
-  bool include_right_lanes;
-  bool include_left_lanes;
-  bool include_opposite_lanes;
-  bool include_conflicting_lanes;
-  std::vector<std::string> boundary_types_to_detect;
-};
-
 class LaneDepartureCheckerNode : public rclcpp::Node
 {
 public:

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp

@@ -0,0 +1,101 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_
+#define AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_
+
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/geometry/pose_deviation.hpp>
+#include <rclcpp/node.hpp>
+
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace autoware::lane_departure_checker
+{
+using autoware::universe_utils::PoseDeviation;
+using autoware_planning_msgs::msg::LaneletRoute;
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using autoware::universe_utils::LinearRing2d;
+
+struct Param
+{
+  static Param init(rclcpp::Node & node);
+  double footprint_margin_scale{};
+  double footprint_extra_margin{};
+  double resample_interval{};
+  double max_deceleration{};
+  double delay_time{};
+  double max_lateral_deviation{};
+  double max_longitudinal_deviation{};
+  double max_yaw_deviation_deg{};
+  double min_braking_distance{};
+  // nearest search to ego
+  double ego_nearest_dist_threshold{};
+  double ego_nearest_yaw_threshold{};
+};
+
+struct NodeParam
+{
+  static NodeParam init(rclcpp::Node & node);
+  bool will_out_of_lane_checker{};
+  bool out_of_lane_checker{};
+  bool boundary_departure_checker{};
+
+  double update_rate{};
+  bool visualize_lanelet{};
+  bool include_right_lanes{};
+  bool include_left_lanes{};
+  bool include_opposite_lanes{};
+  bool include_conflicting_lanes{};
+  std::vector<std::string> boundary_types_to_detect{};
+};
+
+struct Input
+{
+  nav_msgs::msg::Odometry::ConstSharedPtr current_odom{};
+  lanelet::LaneletMapPtr lanelet_map{};
+  LaneletRoute::ConstSharedPtr route{};
+  lanelet::ConstLanelets route_lanelets{};
+  lanelet::ConstLanelets shoulder_lanelets{};
+  Trajectory::ConstSharedPtr reference_trajectory{};
+  Trajectory::ConstSharedPtr predicted_trajectory{};
+  std::vector<std::string> boundary_types_to_detect{};
+};
+
+struct Output
+{
+  std::map<std::string, double> processing_time_map{};
+  bool will_leave_lane{};
+  bool is_out_of_lane{};
+  bool will_cross_boundary{};
+  PoseDeviation trajectory_deviation{};
+  lanelet::ConstLanelets candidate_lanelets{};
+  TrajectoryPoints resampled_trajectory{};
+  std::vector<LinearRing2d> vehicle_footprints{};
+  std::vector<LinearRing2d> vehicle_passing_areas{};
+};
+}  // namespace autoware::lane_departure_checker
+
+#endif  // AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -126,48 +126,19 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
 : Node("lane_departure_checker_node", options)
 {
   using std::placeholders::_1;
-
-  // Enable feature
-  node_param_.will_out_of_lane_checker = declare_parameter<bool>("will_out_of_lane_checker");
-  node_param_.out_of_lane_checker = declare_parameter<bool>("out_of_lane_checker");
-  node_param_.boundary_departure_checker = declare_parameter<bool>("boundary_departure_checker");
-
-  // Node Parameter
-  node_param_.update_rate = declare_parameter<double>("update_rate");
-  node_param_.visualize_lanelet = declare_parameter<bool>("visualize_lanelet");
-  node_param_.include_right_lanes = declare_parameter<bool>("include_right_lanes");
-  node_param_.include_left_lanes = declare_parameter<bool>("include_left_lanes");
-  node_param_.include_opposite_lanes = declare_parameter<bool>("include_opposite_lanes");
-  node_param_.include_conflicting_lanes = declare_parameter<bool>("include_conflicting_lanes");
-
-  // Boundary_departure_checker
-  node_param_.boundary_types_to_detect =
-    declare_parameter<std::vector<std::string>>("boundary_types_to_detect");
+  node_param_ = NodeParam::init(*this);
+  param_ = Param::init(*this);
 
   // Vehicle Info
   const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
   vehicle_length_m_ = vehicle_info.vehicle_length_m;
 
-  // Core Parameter
-  param_.footprint_margin_scale = declare_parameter<double>("footprint_margin_scale");
-  param_.footprint_extra_margin = declare_parameter<double>("footprint_extra_margin");
-  param_.resample_interval = declare_parameter<double>("resample_interval");
-  param_.max_deceleration = declare_parameter<double>("max_deceleration");
-  param_.delay_time = declare_parameter<double>("delay_time");
-  param_.max_lateral_deviation = declare_parameter<double>("max_lateral_deviation");
-  param_.max_longitudinal_deviation = declare_parameter<double>("max_longitudinal_deviation");
-  param_.max_yaw_deviation_deg = declare_parameter<double>("max_yaw_deviation_deg");
-  param_.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
-  param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
-  param_.min_braking_distance = declare_parameter<double>("min_braking_distance");
-
   // Parameter Callback
   set_param_res_ =
     add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1));
 
   // Core
-  lane_departure_checker_ = std::make_unique<LaneDepartureChecker>();
-  lane_departure_checker_->setParam(param_, vehicle_info);
+  lane_departure_checker_ = std::make_unique<LaneDepartureChecker>(param_, vehicle_info);
 
   // Publisher
   processing_time_publisher_ =

control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp

@@ -0,0 +1,59 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/lane_departure_checker/parameters.hpp"
+
+#include <autoware/universe_utils/ros/parameter.hpp>
+#include <rclcpp/node.hpp>
+
+#include <string>
+
+namespace autoware::lane_departure_checker
+{
+using autoware::universe_utils::getOrDeclareParameter;
+
+Param Param::init(rclcpp::Node & node)
+{
+  Param p;
+  p.footprint_margin_scale = getOrDeclareParameter<double>(node, "footprint_margin_scale");
+  p.footprint_extra_margin = getOrDeclareParameter<double>(node, "footprint_extra_margin");
+  p.resample_interval = getOrDeclareParameter<double>(node, "resample_interval");
+  p.max_deceleration = getOrDeclareParameter<double>(node, "max_deceleration");
+  p.delay_time = getOrDeclareParameter<double>(node, "delay_time");
+  p.max_lateral_deviation = getOrDeclareParameter<double>(node, "max_lateral_deviation");
+  p.max_longitudinal_deviation = getOrDeclareParameter<double>(node, "max_longitudinal_deviation");
+  p.max_yaw_deviation_deg = getOrDeclareParameter<double>(node, "max_yaw_deviation_deg");
+  p.min_braking_distance = getOrDeclareParameter<double>(node, "min_braking_distance");
+  p.ego_nearest_dist_threshold = getOrDeclareParameter<double>(node, "ego_nearest_dist_threshold");
+  p.ego_nearest_yaw_threshold = getOrDeclareParameter<double>(node, "ego_nearest_yaw_threshold");
+  return p;
+}
+
+NodeParam NodeParam::init(rclcpp::Node & node)
+{
+  NodeParam p;
+  p.will_out_of_lane_checker = getOrDeclareParameter<bool>(node, "will_out_of_lane_checker");
+  p.out_of_lane_checker = getOrDeclareParameter<bool>(node, "out_of_lane_checker");
+  p.boundary_departure_checker = getOrDeclareParameter<bool>(node, "boundary_departure_checker");
+  p.update_rate = getOrDeclareParameter<double>(node, "update_rate");
+  p.visualize_lanelet = getOrDeclareParameter<bool>(node, "visualize_lanelet");
+  p.include_right_lanes = getOrDeclareParameter<bool>(node, "include_right_lanes");
+  p.include_left_lanes = getOrDeclareParameter<bool>(node, "include_left_lanes");
+  p.include_opposite_lanes = getOrDeclareParameter<bool>(node, "include_opposite_lanes");
+  p.include_conflicting_lanes = getOrDeclareParameter<bool>(node, "include_conflicting_lanes");
+  p.boundary_types_to_detect =
+    getOrDeclareParameter<std::vector<std::string>>(node, "boundary_types_to_detect");
+  return p;
+}
+}  // namespace autoware::lane_departure_checker

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

@@ -564,13 +564,12 @@ int main(int argc, char ** argv)
       node.get(), "goal_planner.");
   goal_planner_parameter.bus_stop_area.use_bus_stop_area = true;
   goal_planner_parameter.lane_departure_check_expansion_margin = 0.2;
-  const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo();
-  autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{};
-  lane_departure_checker.setVehicleInfo(vehicle_info);
   autoware::lane_departure_checker::Param lane_departure_checker_params;
   lane_departure_checker_params.footprint_extra_margin =
     goal_planner_parameter.lane_departure_check_expansion_margin;
-  lane_departure_checker.setParam(lane_departure_checker_params);
+  autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker(
+    lane_departure_checker_params, vehicle_info);
+
   const auto footprint = vehicle_info.createFootprint();
   autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint);
   auto goal_candidates = goal_searcher.search(planner_data);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp

@@ -573,12 +573,11 @@ int main(int argc, char ** argv)
     autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters(
       node.get(), "goal_planner.");
   const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo();
-  autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{};
-  lane_departure_checker.setVehicleInfo(vehicle_info);
   autoware::lane_departure_checker::Param lane_departure_checker_params;
   lane_departure_checker_params.footprint_extra_margin =
     goal_planner_parameter.lane_departure_check_expansion_margin;
-  lane_departure_checker.setParam(lane_departure_checker_params);
+  autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker(
+    lane_departure_checker_params, vehicle_info);
   const auto footprint = vehicle_info.createFootprint();
   autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint);
   auto goal_candidates = goal_searcher.search(planner_data);