-
Notifications
You must be signed in to change notification settings - Fork 678
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor(lane_departure_checker): improve LaneDepartureChecker initia…
…lization and parameter handling (#9791) * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: Kyoichi Sugahara <[email protected]>
- Loading branch information
1 parent
4dc7201
commit 485ccc6
Showing
12 changed files
with
230 additions
and
223 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
101 changes: 101 additions & 0 deletions
101
...ol/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
59 changes: 59 additions & 0 deletions
59
control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.