From ad3806107b609b12d15efc45d3b6b44d5277b968 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 5 Jul 2024 11:37:50 +0900 Subject: [PATCH 01/33] refactor(lane_change): move struct to lane change namespace (#7841) * move struct to lane change namespace Signed-off-by: Muhammad Zulfaqar Azmi * Revert "move struct to lane change namespace" This reverts commit 306984a76103c427732f170a6f7eb5f94e895b0b. Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../utils/base_class.hpp | 19 +++- .../utils/data_structs.hpp | 102 ++++++++++++++---- .../utils/path.hpp | 4 +- 3 files changed, 99 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index e8e83a0703a76..9df19c4de6834 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -50,7 +50,10 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, + common_data_ptr_{std::make_shared()}, + direction_{direction}, + type_{type} { } @@ -151,7 +154,18 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) { planner_data_ = data; } + void setData(const std::shared_ptr & data) + { + planner_data_ = data; + if (!common_data_ptr_->bpp_param_ptr) { + common_data_ptr_->bpp_param_ptr = + std::make_shared(data->parameters); + } + common_data_ptr_->self_odometry_ptr = data->self_odometry; + common_data_ptr_->route_handler_ptr = data->route_handler; + common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->direction = direction_; + } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } @@ -219,6 +233,7 @@ class LaneChangeBase std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + lane_change::CommonDataPtr common_data_ptr_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index ffd2754acc38f..2fdf7c6b550a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -17,16 +17,28 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include +#include #include +#include + #include #include +#include #include #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; +using utils::path_safety_checker::ExtendedPredictedObjects; + struct LateralAccelerationMap { std::vector base_vel; @@ -68,7 +80,7 @@ struct LateralAccelerationMap } }; -struct LaneChangeCancelParameters +struct CancelParameters { bool enable_on_prepare_phase{true}; bool enable_on_lane_changing_phase{false}; @@ -83,7 +95,7 @@ struct LaneChangeCancelParameters int unsafe_hysteresis_threshold{2}; }; -struct LaneChangeParameters +struct Parameters { // trajectory generation double backward_lane_length{200.0}; @@ -92,8 +104,8 @@ struct LaneChangeParameters int lateral_acc_sampling_num{10}; // lane change parameters - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; @@ -143,7 +155,7 @@ struct LaneChangeParameters utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel{}; + CancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; @@ -151,32 +163,32 @@ struct LaneChangeParameters bool publish_debug_marker{false}; }; -enum class LaneChangeStates { +enum class States { Normal = 0, Cancel, Abort, Stop, }; -struct LaneChangePhaseInfo +struct PhaseInfo { double prepare{0.0}; double lane_changing{0.0}; [[nodiscard]] double sum() const { return prepare + lane_changing; } - LaneChangePhaseInfo(const double _prepare, const double _lane_changing) + PhaseInfo(const double _prepare, const double _lane_changing) : prepare(_prepare), lane_changing(_lane_changing) { } }; -struct LaneChangeInfo +struct Info { - LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0}; - LaneChangePhaseInfo velocity{0.0, 0.0}; - LaneChangePhaseInfo duration{0.0, 0.0}; - LaneChangePhaseInfo length{0.0, 0.0}; + PhaseInfo longitudinal_acceleration{0.0, 0.0}; + PhaseInfo velocity{0.0, 0.0}; + PhaseInfo duration{0.0, 0.0}; + PhaseInfo length{0.0, 0.0}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; @@ -190,22 +202,19 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeLanesFilteredObjects +struct LanesObjects { - utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; + ExtendedPredictedObjects current_lane{}; + ExtendedPredictedObjects target_lane{}; + ExtendedPredictedObjects other_lane{}; }; -enum class LaneChangeModuleType { +enum class ModuleType { NORMAL = 0, EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; -} // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::lane_change -{ struct PathSafetyStatus { bool is_safe{true}; @@ -218,6 +227,55 @@ struct LanesPolygon std::optional target; std::vector target_backward; }; + +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + +struct CommonData +{ + std::shared_ptr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + std::shared_ptr bpp_param_ptr; + std::shared_ptr lc_param_ptr; + Lanes lanes; + Direction direction; + + [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + + [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using CommonDataPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change +namespace autoware::behavior_path_planner +{ +using LaneChangeModuleType = lane_change::ModuleType; +using LaneChangeParameters = lane_change::Parameters; +using LaneChangeStates = lane_change::States; +using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangeInfo = lane_change::Info; +using LaneChangeLanesFilteredObjects = lane_change::LanesObjects; +using LateralAccelerationMap = lane_change::LateralAccelerationMap; +} // namespace autoware::behavior_path_planner + #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 77c603e3bd975..97b5c621deea5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -31,8 +31,8 @@ struct LaneChangePath { PathWithLaneId path{}; ShiftedPath shifted_path{}; - PathWithLaneId prev_path{}; - LaneChangeInfo info{}; + LaneChangeInfo info; + bool is_safe{false}; }; using LaneChangePaths = std::vector; From 5eb577ea4c5a21637bc74f4e034799a6fdc0a113 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 5 Jul 2024 12:10:51 +0900 Subject: [PATCH 02/33] fix(image_projection_based_fusion): segmentation pointcloud fusion param update (#7858) --- .../segmentation_pointcloud_fusion.param.yaml | 53 +++---- .../docs/segmentation-pointcloud-fusion.md | 4 +- .../segmentation_pointcloud_fusion/node.hpp | 9 +- ...segmentation_pointcloud_fusion.schema.json | 134 ++++++++++++++++++ .../segmentation_pointcloud_fusion/node.cpp | 13 +- 5 files changed, 171 insertions(+), 42 deletions(-) create mode 100644 perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 2120a909cd672..fdabb0c7055d8 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -1,39 +1,24 @@ /**: ros__parameters: # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: - [ - true, # road - true, # sidewalk - true, # building - true, # wall - true, # fence - true, # pole - true, # traffic_light - true, # traffic_sign - true, # vegetation - true, # terrain - true, # sky - false, # person - false, # ride - false, # car - false, # truck - false, # bus - false, # train - false, # motorcycle - false, # bicycle - false, # others - ] - # the maximum distance of pointcloud to be applied filter, - # this is selected based on semantic segmentation model accuracy, - # calibration accuracy and unknown reaction distance - filter_distance_threshold: 60.0 + UNKNOWN: false + BUILDING: true + WALL: true + OBSTACLE: false + TRAFFIC_LIGHT: false + TRAFFIC_SIGN: false + PERSON: false + VEHICLE: false + BIKE: false + ROAD: true + SIDEWALK: false + ROAD_PAINT: false + CURBSTONE: false + CROSSWALK: false + VEGETATION: true + SKY: false - # debug - debug_mode: false - filter_scope_min_x: -100.0 - filter_scope_max_x: 100.0 - filter_scope_min_y: -100.0 - filter_scope_max_y: 100.0 - filter_scope_min_z: -100.0 - filter_scope_max_z: 100.0 + # the maximum distance of pointcloud to be applied filter + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index d59e804f1228c..3c469ac15c6e7 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -32,9 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Core Parameters -| Name | Type | Description | -| ------------- | ---- | ------------------------ | -| `rois_number` | int | the number of input rois | +{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 26bcfd5ed802a..89b33775f7898 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #if __has_include() @@ -36,7 +37,13 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_pointcloud_ptr_; std::vector filter_semantic_label_target_; float filter_distance_threshold_; - /* data */ + // declare list of semantic label target, depend on trained data of yolox segmentation model + std::vector> filter_semantic_label_target_list_ = { + {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, + {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, + {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; + public: explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f5ab1be5eac34 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -0,0 +1,134 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Segmentation Pointcloud Fusion Node", + "type": "object", + "definitions": { + "segmentation_pointcloud_fusion": { + "type": "object", + "properties": { + "filter_semantic_label_target": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "description": "If true, UNKNOWN class of semantic will be filtered.", + "default": false + }, + "BUILDING": { + "type": "boolean", + "description": "If true, BUILDING class of semantic will be filtered.", + "default": true + }, + "WALL": { + "type": "boolean", + "description": "If true, WALL class of semantic will be filtered.", + "default": true + }, + "OBSTACLE": { + "type": "boolean", + "description": "If true, OBSTACLE class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_LIGHT": { + "type": "boolean", + "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_SIGN": { + "type": "boolean", + "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", + "default": false + }, + "PERSON": { + "type": "boolean", + "description": "If true, PERSON class of semantic will be filtered.", + "default": false + }, + "VEHICLE": { + "type": "boolean", + "description": "If true, VEHICLE class of semantic will be filtered.", + "default": false + }, + "BIKE": { + "type": "boolean", + "description": "If true, BIKE class of semantic will be filtered.", + "default": false + }, + "ROAD": { + "type": "boolean", + "description": "If true, ROAD class of semantic will be filtered.", + "default": true + }, + "SIDEWALK": { + "type": "boolean", + "description": "If true, SIDEWALK class of semantic will be filtered.", + "default": false + }, + "ROAD_PAINT": { + "type": "boolean", + "description": "If true, ROAD_PAINT class of semantic will be filtered.", + "default": false + }, + "CURBSTONE": { + "type": "boolean", + "description": "If true, CURBSTONE class of semantic will be filtered.", + "default": false + }, + "CROSSWALK": { + "type": "boolean", + "description": "If true, CROSSWALK class of semantic will be filtered.", + "default": false + }, + "VEGETATION": { + "type": "boolean", + "description": "If true, VEGETATION class of semantic will be filtered.", + "default": true + }, + "SKY": { + "type": "boolean", + "description": "If true, SKY class of semantic will be filtered.", + "default": false + } + }, + "required": [ + "UNKNOWN", + "BUILDING", + "WALL", + "OBSTACLE", + "TRAFFIC_LIGHT", + "TRAFFIC_SIGN", + "PERSON", + "VEHICLE", + "BIKE", + "ROAD", + "SIDEWALK", + "ROAD_PAINT", + "CURBSTONE", + "CROSSWALK", + "VEGETATION", + "SKY" + ] + }, + "filter_distance_threshold": { + "type": "number", + "description": "A maximum distance of pointcloud to apply filter [m].", + "default": 60.0, + "minimum": 0.0 + } + }, + "required": ["filter_semantic_label_target", "filter_distance_threshold"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segmentation_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index a3117125a46b1..9a983252af436 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -31,8 +31,13 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio : FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); - filter_semantic_label_target_ = - declare_parameter>("filter_semantic_label_target"); + for (auto & item : filter_semantic_label_target_list_) { + item.second = declare_parameter("filter_semantic_label_target." + item.first); + } + for (const auto & item : filter_semantic_label_target_list_) { + RCLCPP_INFO( + this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); + } } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -129,12 +134,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( static_cast(projected_point.y()), static_cast(projected_point.x())); - if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + if (static_cast(semantic_id) >= filter_semantic_label_target_list_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } - if (!filter_semantic_label_target_.at(semantic_id)) { + if (!filter_semantic_label_target_list_.at(semantic_id).second) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } From 134b5c208eec3f0e11fd6d706ada3dd592b45a3c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 5 Jul 2024 12:49:24 +0900 Subject: [PATCH 03/33] fix(static_obstacle_avoidance): ignore pedestrian/cyclist who is not on road edge (#7850) * fix(static_obstacle_avoidance): ignore pedestrian/cyclist who is not on road edge Signed-off-by: satoshi-ota * docs(static_obstacle_avoidance): update flowchart Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md Co-authored-by: Go Sakayori --------- Signed-off-by: satoshi-ota Co-authored-by: Go Sakayori --- .../README.md | 21 ++++++++++ .../src/utils.cpp | 41 +++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 40d36898b2309..8e20cf2c9f660 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -659,6 +659,27 @@ if(isWithinCrosswalk()) then (yes) stop else (\n no) endif + +if(is object within intersection?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif + +if(is object on right side of the ego path?) then (yes) +if(are there adjacent lanes on right side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +else (\n no) +if(are there adjacent lanes on left side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +endif + #FF006C :return true; stop } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index d454a97400e78..30d1aad16374b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -809,6 +809,45 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + if (object.is_within_intersection) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "object is within intersection. don't have to avoid it."); + return false; + } + + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto right_opposite_lanes = + planner_data->route_handler->getRightOppositeLanelets(object.overhang_lanelet); + if (!right_opposite_lanes.empty() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_opposite_lanes = + planner_data->route_handler->getLeftOppositeLanelets(object.overhang_lanelet); + if (!left_opposite_lanes.empty() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + return true; } @@ -1840,6 +1879,8 @@ void filterTargetObjects( continue; } } else { + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); From 0f60c62d8bcc6f1c6a5cef67c7bc8626c06ddb1f Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 5 Jul 2024 13:13:57 +0900 Subject: [PATCH 04/33] feat(planning_evaluator): add planning evaluator polling sub (#7827) * WIP add polling subs Signed-off-by: Daniel Sanchez * WIP Signed-off-by: Daniel Sanchez * update functions Signed-off-by: Daniel Sanchez * remove semicolon Signed-off-by: Daniel Sanchez * use last data for modified goal Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../planning_evaluator_node.hpp | 46 ++++-- .../src/planning_evaluator_node.cpp | 146 +++++++++--------- 2 files changed, 106 insertions(+), 86 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 74523398c3a77..91dc0386ae6c6 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -70,7 +70,8 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a trajectory * @param [in] traj_msg received trajectory message */ - void onTrajectory(const Trajectory::ConstSharedPtr traj_msg); + void onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief callback on receiving a reference trajectory @@ -88,7 +89,9 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a modified goal * @param [in] modified_goal_msg received modified goal message */ - void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); + void onModifiedGoal( + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish the given metric statistic @@ -99,26 +102,43 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief publish current ego lane info */ - DiagnosticStatus generateLaneletDiagnosticStatus(); + DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish current ego kinematic state */ DiagnosticStatus generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped); + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); private: static bool isFinite(const TrajectoryPoint & p); - void publishModifiedGoalDeviationMetrics(); - // update Route Handler + + /** + * @brief update route handler data + */ void getRouteData(); + /** + * @brief fetch data and publish diagnostics + */ + void onTimer(); + + /** + * @brief fetch topic data + */ + void fetchData(); + // ROS - rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr ref_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr modified_goal_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber ref_sub_{ + this, "~/input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber modified_goal_sub_{ + this, "~/input/modified_goal"}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ @@ -131,6 +151,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; + DiagnosticArray metrics_msg_; // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -142,8 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; - Odometry::ConstSharedPtr ego_state_ptr_; - PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; + rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5e959aed123af..9b65dbbc0b89e 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -35,26 +35,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op : Node("planning_evaluator", node_options) { using std::placeholders::_1; - - traj_sub_ = create_subscription( - "~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1)); - - ref_sub_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1)); - - objects_sub_ = create_subscription( - "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); - - modified_goal_sub_ = create_subscription( - "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); - - odom_sub_ = create_subscription( - "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); - tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); + // Timer callback to publish evaluator diagnostics + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&PlanningEvaluatorNode::onTimer, this)); // Parameters metrics_calculator_.parameters.trajectory.min_point_dist_m = declare_parameter("trajectory.min_point_dist_m"); @@ -133,9 +120,10 @@ void PlanningEvaluatorNode::getRouteData() } } -DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() +DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( + const Odometry::ConstSharedPtr ego_state_ptr) { - const auto & ego_pose = ego_state_ptr_->pose.pose; + const auto & ego_pose = ego_state_ptr->pose.pose; const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); @@ -166,14 +154,14 @@ DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() } DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped) + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) { DiagnosticStatus status; status.name = "kinematic_state"; status.level = status.OK; diagnostic_msgs::msg::KeyValue key_value; key_value.key = "vel"; - key_value.value = std::to_string(ego_state_ptr_->twist.twist.linear.x); + key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); status.values.push_back(key_value); key_value.key = "acc"; const auto & acc = accel_stamped.accel.accel.linear.x; @@ -220,9 +208,40 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( return status; } -void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) +void PlanningEvaluatorNode::onTimer() +{ + metrics_msg_.header.stamp = now(); + + const auto ego_state_ptr = odometry_sub_.takeData(); + onOdometry(ego_state_ptr); + { + const auto objects_msg = objects_sub_.takeData(); + onObjects(objects_msg); + } + + { + const auto ref_traj_msg = ref_sub_.takeData(); + onReferenceTrajectory(ref_traj_msg); + } + + { + const auto traj_msg = traj_sub_.takeData(); + onTrajectory(traj_msg, ego_state_ptr); + } + { + const auto modified_goal_msg = modified_goal_sub_.takeData(); + onModifiedGoal(modified_goal_msg, ego_state_ptr); + } + if (!metrics_msg_.status.empty()) { + metrics_pub_->publish(metrics_msg_); + } + metrics_msg_ = DiagnosticArray{}; +} + +void PlanningEvaluatorNode::onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr) { - if (!ego_state_ptr_) { + if (!ego_state_ptr || !traj_msg) { return; } @@ -231,8 +250,6 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m stamps_.push_back(traj_msg->header.stamp); } - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); if (!metric_stat) { @@ -244,88 +261,71 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m } if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } + metrics_calculator_.setPreviousTrajectory(*traj_msg); auto runtime = (now() - start).seconds(); RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); } void PlanningEvaluatorNode::onModifiedGoal( - const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg) + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr) { - modified_goal_ptr_ = modified_goal_msg; - if (ego_state_ptr_) { - publishModifiedGoalDeviationMetrics(); - } -} - -void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) -{ - ego_state_ptr_ = odometry_msg; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); - { - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); - - getRouteData(); - if (route_handler_.isHandlerReady() && ego_state_ptr_) { - metrics_msg.status.push_back(generateLaneletDiagnosticStatus()); - } - - const auto acc = accel_sub_.takeData(); - - if (acc && ego_state_ptr_) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc)); - } - - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } - } - - if (modified_goal_ptr_) { - publishModifiedGoalDeviationMetrics(); + if (!modified_goal_msg || !ego_state_ptr) { + return; } -} - -void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics() -{ auto start = now(); - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate( - Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose); + Metric(metric), modified_goal_msg->pose, ego_state_ptr->pose.pose); if (!metric_stat) { continue; } metric_stats_[static_cast(metric)].push_back(*metric_stat); if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } auto runtime = (now() - start).seconds(); RCLCPP_DEBUG( get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms", runtime * 1e3); } +void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) +{ + if (!odometry_msg) return; + metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + { + getRouteData(); + if (route_handler_.isHandlerReady() && odometry_msg) { + metrics_msg_.status.push_back(generateLaneletDiagnosticStatus(odometry_msg)); + } + + const auto acc_msg = accel_sub_.takeData(); + if (acc_msg && odometry_msg) { + metrics_msg_.status.push_back(generateKinematicStateDiagnosticStatus(*acc_msg, odometry_msg)); + } + } +} + void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) { + if (!traj_msg) { + return; + } metrics_calculator_.setReferenceTrajectory(*traj_msg); } void PlanningEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { + if (!objects_msg) { + return; + } metrics_calculator_.setPredictedObjects(*objects_msg); } From 6c944bfa4ac7eabedc076286458a2b391af11747 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 5 Jul 2024 13:14:39 +0900 Subject: [PATCH 05/33] perf(map_based_prediction): remove query on all fences linestrings (#7237) Signed-off-by: Maxime CLEMENT --- .../src/map_based_prediction_node.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 2b8358eb52e4e..4f223d5c99bd6 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -1309,10 +1309,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - const lanelet::ConstLineStrings3d & all_fences = - lanelet::utils::query::getAllFences(lanelet_map_ptr_); - for (const auto & fence_line : all_fences) { - if (doesPathCrossFence(predicted_path, fence_line)) { + lanelet::BasicLineString2d predicted_path_ls; + for (const auto & p : predicted_path.path) + predicted_path_ls.emplace_back(p.position.x, p.position.y); + const auto candidates = + lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); + for (const auto & candidate : candidates) { + const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) { return true; } } From 589c88ba8594feea56aff3841f263ef3be1fccca Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Fri, 5 Jul 2024 14:17:47 +0900 Subject: [PATCH 06/33] fix(yabloc_image_processing): fix shadowFunction (#7865) * fix(yabloc_image_processing): fix shadowFunction Signed-off-by: veqcc * fix Signed-off-by: veqcc * style(pre-commit): autofix --------- Signed-off-by: veqcc Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/undistort/undistort_node.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 46f5165c5f4e2..4c33377c8ed0f 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -53,14 +53,13 @@ class UndistortNode : public rclcpp::Node qos = rclcpp::QoS(10).durability_volatile().best_effort(); } - auto on_image = std::bind(&UndistortNode::on_image, this, _1); - auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); - auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription( + "~/input/image_raw", qos, std::bind(&UndistortNode::on_image, this, _1)); sub_compressed_image_ = create_subscription( - "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); - - sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); + "~/input/image_raw/compressed", qos, + std::bind(&UndistortNode::on_compressed_image, this, _1)); + sub_info_ = create_subscription( + "~/input/camera_info", qos, std::bind(&UndistortNode::on_info, this, _1)); pub_info_ = create_publisher("~/output/resized_info", 10); pub_image_ = create_publisher("~/output/resized_image", 10); From 22daaeb0378087b30b688a4c10c2658cbe2846f1 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Fri, 5 Jul 2024 14:31:00 +0900 Subject: [PATCH 07/33] chore(ci): remove unnecessary cppcheck suppression (#7862) Signed-off-by: Ryuta Kambe --- .cppcheck_suppressions | 1 - 1 file changed, 1 deletion(-) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 1407a01c4054a..7c921dac7db4c 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -15,7 +15,6 @@ missingIncludeSystem noConstructor passedByValue redundantInitialization -shadowFunction shadowVariable // cspell: ignore uninit uninitMemberVar From 3627afcafbf807c2a9bc7bae41cc1ac7f11eafa1 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Fri, 5 Jul 2024 15:06:44 +0900 Subject: [PATCH 08/33] fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (#7855) * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions Signed-off-by: Ryuta Kambe * style(pre-commit): autofix --------- Signed-off-by: Ryuta Kambe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/fake_test_node/test/test_fake_test_node.cpp | 4 +--- common/osqp_interface/test/test_osqp_interface.cpp | 1 - common/qp_interface/test/test_osqp_interface.cpp | 1 - common/qp_interface/test/test_proxqp_interface.cpp | 1 - 4 files changed, 1 insertion(+), 6 deletions(-) diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index 163bd761407c5..77e9294cb48c2 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -96,9 +96,7 @@ TEST_F(FakeNodeFixture, Test) } INSTANTIATE_TEST_SUITE_P( - FakeNodeFixtureTests, FakeNodeFixtureParametrized, - // cppcheck-suppress syntaxError // cppcheck doesn't like the trailing comma. - ::testing::Values(-5, 0, 42)); + FakeNodeFixtureTests, FakeNodeFixtureParametrized, ::testing::Values(-5, 0, 42)); /// @test Test that we can use a parametrized test. TEST_P(FakeNodeFixtureParametrized, Test) diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index caa89c79d08ea..4644ec6c3e019 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -37,7 +37,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { using autoware::common::osqp::calCSCMatrix; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index e5d7041469289..5ce1f0eb064a1 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -37,7 +37,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { using qp::calCSCMatrix; diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index 96466665d5172..c28678e5c872a 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -38,7 +38,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestProxqpInterface, BasicQp) { auto check_result = [](const auto & solution) { From 5be7627b0973c572c2ce593c5b318df6dfd25775 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 5 Jul 2024 16:37:00 +0900 Subject: [PATCH 09/33] feat(mission_planner): reroute in manual driving (#7842) * feat(mission_planner): reroute in manual driving Signed-off-by: Fumiya Watanabe * docs(mission_planner): update document Signed-off-by: Fumiya Watanabe * feat(mission_planner): fix operation mode state receiving check Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- planning/autoware_mission_planner/README.md | 10 ++-- .../launch/mission_planner.launch.xml | 1 + .../src/mission_planner/mission_planner.cpp | 50 ++++++++++++++++--- .../src/mission_planner/mission_planner.hpp | 5 ++ 4 files changed, 54 insertions(+), 12 deletions(-) diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index 2f912e30b8246..452d0a64d5c9c 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -47,10 +47,11 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| --------------------- | ----------------------------------- | ---------------------- | -| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | -| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | ### Publications @@ -170,6 +171,7 @@ To calculate `route_lanelets`, ### Rerouting Reroute here means changing the route while driving. Unlike route setting, it is required to keep a certain distance from vehicle to the point where the route is changed. +If the ego vehicle is not on autonomous driving state, the safety checking process will be skipped. ![rerouting_safety](./media/rerouting_safety.svg) diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/planning/autoware_mission_planner/launch/mission_planner.launch.xml index 655662c392213..8d77e417a6379 100644 --- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml +++ b/planning/autoware_mission_planner/launch/mission_planner.launch.xml @@ -10,6 +10,7 @@ + diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 0df01257f049e..2b1b943e002a8 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -55,6 +55,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/operation_mode_state", rclcpp::QoS(1), + std::bind(&MissionPlanner::on_operation_mode_state, this, _1)); sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); @@ -130,6 +133,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } +void MissionPlanner::on_operation_mode_state(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -222,10 +230,23 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -238,7 +259,7 @@ void MissionPlanner::on_set_lanelet_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( @@ -271,10 +292,23 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -287,7 +321,7 @@ void MissionPlanner::on_set_waypoint_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 1a04a91c14ba3..1181ef54273ae 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,7 @@ namespace autoware::mission_planner { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; @@ -85,18 +87,21 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; autoware::universe_utils::InterProcessPollingSubscriber sub_reroute_availability_{this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; + OperationModeState::ConstSharedPtr operation_mode_state_; LaneletMapBin::ConstSharedPtr map_ptr_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_operation_mode_state(const OperationModeState::ConstSharedPtr msg); void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); From 859da5a8426044597e46bab36db48b3f37c106ef Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 5 Jul 2024 16:47:02 +0900 Subject: [PATCH 10/33] refactor(map_loader): apply static analysis (#7845) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../map_loader/lanelet2_map_loader_node.hpp | 7 +- .../lanelet2_map_visualization_node.hpp | 2 +- .../lanelet2_local_projector.hpp | 2 +- .../lanelet2_map_loader_node.cpp | 10 +- .../lanelet2_map_visualization_node.cpp | 160 ++++++++++-------- .../differential_map_loader_module.cpp | 30 ++-- .../differential_map_loader_module.hpp | 12 +- .../partial_map_loader_module.cpp | 33 ++-- .../partial_map_loader_module.hpp | 12 +- .../pointcloud_map_loader_module.cpp | 8 +- .../pointcloud_map_loader_module.hpp | 2 +- .../pointcloud_map_loader_node.cpp | 33 ++-- .../pointcloud_map_loader_node.hpp | 4 +- .../selected_map_loader_module.cpp | 28 +-- .../selected_map_loader_module.hpp | 6 +- .../src/pointcloud_map_loader/utils.cpp | 36 ++-- .../src/pointcloud_map_loader/utils.hpp | 10 +- .../test/test_cylinder_box_overlap.cpp | 6 +- .../test/test_load_pcd_metadata.cpp | 6 +- .../test_pointcloud_map_loader_module.cpp | 4 +- .../test/test_replace_with_absolute_path.cpp | 4 +- 21 files changed, 217 insertions(+), 198 deletions(-) diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 4e85ddec056c1..8bfdbc5d5560c 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -27,9 +27,6 @@ #include #include -using autoware_map_msgs::msg::LaneletMapBin; -using tier4_map_msgs::msg::MapProjectorInfo; - class Lanelet2MapLoaderNode : public rclcpp::Node { public: @@ -38,7 +35,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static LaneletMapBin create_map_bin_msg( + static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +45,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index cb640e4dc83d5..049d714ec452a 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -34,7 +34,7 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node bool viz_lanelets_centerline_; - void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); + void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp index 225445d17bfa1..d73ec0d1ee06e 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -30,7 +30,7 @@ class LocalProjector : public Projector return BasicPoint3d{0.0, 0.0, gps.ele}; } - GPSPoint reverse(const BasicPoint3d & point) const override + [[nodiscard]] GPSPoint reverse(const BasicPoint3d & point) const override { return GPSPoint{0.0, 0.0, point.z()}; } diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 7b712c7c281a2..e2b2a052619e4 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -51,6 +51,9 @@ #include +using autoware_map_msgs::msg::LaneletMapBin; +using tier4_map_msgs::msg::MapProjectorInfo; + Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { @@ -105,13 +108,13 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = geography_utils::get_lanelet2_projector(projector_info); - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } } else { const lanelet::projection::LocalProjector projector; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (!errors.empty()) { for (const auto & error : errors) { @@ -150,7 +153,8 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { - std::string format_version{}, map_version{}; + std::string format_version{}; + std::string map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 87319222001d9..4bff12c640c30 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -51,18 +51,18 @@ namespace { -void insertMarkerArray( +void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; + cl->r = static_cast(r); + cl->g = static_cast(g); + cl->b = static_cast(b); + cl->a = static_cast(a); } } // namespace @@ -75,13 +75,13 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); + std::bind(&Lanelet2MapVisualizationNode::on_map_bin, this, _1)); pub_marker_ = this->create_publisher( "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); } -void Lanelet2MapVisualizationNode::onMapBin( +void Lanelet2MapVisualizationNode::on_map_bin( const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -103,8 +103,6 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); - std::vector tl_reg_elems = - lanelet::utils::query::trafficLights(all_lanelets); std::vector aw_tl_reg_elems = lanelet::utils::query::autowareTrafficLights(all_lanelets); std::vector da_reg_elems = @@ -133,140 +131,158 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); - std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, - cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, - cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; - setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); - setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); - setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); - setColor(&cl_partitions, 0.25, 0.25, 0.25, 0.999); - setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); - setColor(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); - setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); - setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); - setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); - setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); - setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); - setColor(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); - setColor(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); - setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); - setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); - setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); - setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); - setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); - setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); + std_msgs::msg::ColorRGBA cl_road; + std_msgs::msg::ColorRGBA cl_shoulder; + std_msgs::msg::ColorRGBA cl_cross; + std_msgs::msg::ColorRGBA cl_partitions; + std_msgs::msg::ColorRGBA cl_pedestrian_markings; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_shoulder_borders; + std_msgs::msg::ColorRGBA cl_stoplines; + std_msgs::msg::ColorRGBA cl_trafficlights; + std_msgs::msg::ColorRGBA cl_detection_areas; + std_msgs::msg::ColorRGBA cl_speed_bumps; + std_msgs::msg::ColorRGBA cl_crosswalks; + std_msgs::msg::ColorRGBA cl_parking_lots; + std_msgs::msg::ColorRGBA cl_parking_spaces; + std_msgs::msg::ColorRGBA cl_lanelet_id; + std_msgs::msg::ColorRGBA cl_obstacle_polygons; + std_msgs::msg::ColorRGBA cl_no_stopping_areas; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_area; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_line; + std_msgs::msg::ColorRGBA cl_no_parking_areas; + std_msgs::msg::ColorRGBA cl_curbstones; + std_msgs::msg::ColorRGBA cl_intersection_area; + set_color(&cl_road, 0.27, 0.27, 0.27, 0.999); + set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5); + set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999); + set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); + set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); + set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); + set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( pedestrian_polygon_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( pedestrian_line_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( road_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( crosswalk_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker( crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( intersection_areas, cl_intersection_area)); diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index a8d380fd81b86..da42389dcc69f 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -14,20 +14,22 @@ #include "differential_map_loader_module.hpp" +#include + DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_differential_pcd_maps_service_ = node->create_service( "service/get_differential_pcd_map", std::bind( - &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + &DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map, this, std::placeholders::_1, std::placeholders::_2)); } -void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const +void DifferentialMapLoaderModule::differential_area_load( + const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids std::vector should_remove(static_cast(cached_ids.size()), true); @@ -36,18 +38,18 @@ void DifferentialMapLoaderModule::differentialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area_info, metadata)) continue; auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); if (id_in_cached_list != cached_ids.end()) { - int index = id_in_cached_list - cached_ids.begin(); + int index = static_cast(id_in_cached_list - cached_ids.begin()); should_remove[index] = false; } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -63,19 +65,19 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } } -bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( +bool DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res) + GetDifferentialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; std::vector cached_ids = req->cached_ids; - differentialAreaLoad(area, cached_ids, res); + differential_area_load(area, cached_ids, res); res->header.frame_id = "map"; return true; } autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( +DifferentialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 7069e1dbdf45c..690ffeca653c8 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -38,7 +38,7 @@ class DifferentialMapLoaderModule public: explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class DifferentialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; - bool onServiceGetDifferentialPointCloudMap( + [[nodiscard]] bool on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res); - void differentialAreaLoad( + GetDifferentialPointCloudMap::Response::SharedPtr res) const; + void differential_area_load( const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 8c9378e9dfadb..62e165dd1005b 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -14,19 +14,22 @@ #include "partial_map_loader_module.hpp" +#include + PartialMapLoaderModule::PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_partial_pcd_maps_service_ = node->create_service( - "service/get_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_partial_pcd_map", + std::bind( + &PartialMapLoaderModule::on_service_get_partial_point_cloud_map, this, std::placeholders::_1, + std::placeholders::_2)); } -void PartialMapLoaderModule::partialAreaLoad( +void PartialMapLoaderModule::partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const + const GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids for (const auto & ele : all_pcd_file_metadata_dict_) { @@ -34,13 +37,13 @@ void PartialMapLoaderModule::partialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area, metadata)) continue; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -50,16 +53,18 @@ void PartialMapLoaderModule::partialAreaLoad( } } -bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res) +bool PartialMapLoaderModule::on_service_get_partial_point_cloud_map( + GetPartialPointCloudMap::Request::SharedPtr req, + GetPartialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; - partialAreaLoad(area, res); + partial_area_load(area, res); res->header.frame_id = "map"; return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithID +PartialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 4d97ab90667ec..ec97661366419 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -38,7 +38,7 @@ class PartialMapLoaderModule public: explicit PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class PartialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_partial_pcd_maps_service_; - bool onServiceGetPartialPointCloudMap( + [[nodiscard]] bool on_service_get_partial_point_cloud_map( GetPartialPointCloudMap::Request::SharedPtr req, - GetPartialPointCloudMap::Response::SharedPtr res); - void partialAreaLoad( + GetPartialPointCloudMap::Response::SharedPtr res) const; + void partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetPartialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index a66f9ee99534c..1754d0b7629a2 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -50,10 +50,10 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( sensor_msgs::msg::PointCloud2 pcd; if (use_downsample) { - const float leaf_size = node->declare_parameter("leaf_size"); - pcd = loadPCDFiles(pcd_paths, leaf_size); + const float leaf_size = static_cast(node->declare_parameter("leaf_size")); + pcd = load_pcd_files(pcd_paths, leaf_size); } else { - pcd = loadPCDFiles(pcd_paths, boost::none); + pcd = load_pcd_files(pcd_paths, boost::none); } if (pcd.width == 0) { @@ -65,7 +65,7 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( pub_pointcloud_map_->publish(pcd); } -sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( +sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const { sensor_msgs::msg::PointCloud2 whole_pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 6a87643b57bff..44f23ded70e37 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -39,7 +39,7 @@ class PointcloudMapLoaderModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles( + [[nodiscard]] sensor_msgs::msg::PointCloud2 load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 349fc2954fe28..c718b25c56694 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -28,7 +28,7 @@ namespace fs = std::filesystem; namespace { -bool isPcdFile(const std::string & p) +bool is_pcd_file(const std::string & p) { if (fs::is_directory(p)) { return false; @@ -36,11 +36,7 @@ bool isPcdFile(const std::string & p) const std::string ext = fs::path(p).extension(); - if (ext != ".pcd" && ext != ".PCD") { - return false; - } - - return true; + return !(ext != ".pcd" && ext != ".PCD"); } } // namespace @@ -48,7 +44,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt : Node("pointcloud_map_loader", options) { const auto pcd_paths = - getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + get_pcd_paths(declare_parameter>("pcd_paths_or_directory")); std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); @@ -68,7 +64,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } // Parse the metadata file and get the map of (absolute pcd path, pcd file metadata) - auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + auto pcd_metadata_dict = get_pcd_metadata(pcd_metadata_path, pcd_paths); if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); @@ -81,14 +77,14 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } } -std::map PointCloudMapLoaderNode::getPCDMetadata( +std::map PointCloudMapLoaderNode::get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { if (fs::exists(pcd_metadata_path)) { std::set missing_pcd_names; - auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + auto pcd_metadata_dict = load_pcd_metadata(pcd_metadata_path); - pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names); + pcd_metadata_dict = replace_with_absolute_path(pcd_metadata_dict, pcd_paths, missing_pcd_names); // Warning if some segments are missing if (!missing_pcd_names.empty()) { @@ -96,7 +92,7 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( oss << "The following segment(s) are missing from the input PCDs: "; - for (auto & fname : missing_pcd_names) { + for (const auto & fname : missing_pcd_names) { oss << std::endl << fname; } @@ -105,7 +101,9 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( } return pcd_metadata_dict; - } else if (pcd_paths.size() == 1) { + } + + if (pcd_paths.size() == 1) { // An exception when using a single file PCD map so that the users do not have to provide // a metadata file. // Note that this should ideally be avoided and thus eventually be removed by someone, until @@ -119,12 +117,11 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( PCDFileMetadata metadata = {}; pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); return std::map{{pcd_path, metadata}}; - } else { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } -std::vector PointCloudMapLoaderNode::getPcdPaths( +std::vector PointCloudMapLoaderNode::get_pcd_paths( const std::vector & pcd_paths_or_directory) const { std::vector pcd_paths; @@ -133,14 +130,14 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); } - if (isPcdFile(p)) { + if (is_pcd_file(p)) { pcd_paths.push_back(p); } if (fs::is_directory(p)) { for (const auto & file : fs::directory_iterator(p)) { const auto filename = file.path().string(); - if (isPcdFile(filename)) { + if (is_pcd_file(filename)) { pcd_paths.push_back(filename); } } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index fd9b297a9e3f4..dbc0d584d347b 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -45,9 +45,9 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr differential_map_loader_; std::unique_ptr selected_map_loader_; - std::vector getPcdPaths( + std::vector get_pcd_paths( const std::vector & pcd_paths_or_directory) const; - std::map getPCDMetadata( + std::map get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp index 3e7b046f9d178..76b56341b8632 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp @@ -13,9 +13,11 @@ // limitations under the License. #include "selected_map_loader_module.hpp" + +#include namespace { -autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( +autoware_map_msgs::msg::PointCloudMapMetaData create_metadata( const std::map & pcd_file_metadata_dict) { autoware_map_msgs::msg::PointCloudMapMetaData metadata_msg; @@ -23,11 +25,10 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( metadata_msg.header.stamp = rclcpp::Clock().now(); for (const auto & ele : pcd_file_metadata_dict) { - std::string path = ele.first; PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = ele.first; autoware_map_msgs::msg::PointCloudMapCellMetaDataWithID cell_metadata_with_id; cell_metadata_with_id.cell_id = map_id; @@ -44,23 +45,24 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( } // namespace SelectedMapLoaderModule::SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_selected_pcd_maps_service_ = node->create_service( - "service/get_selected_pcd_map", std::bind( - &SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_selected_pcd_map", + std::bind( + &SelectedMapLoaderModule::on_service_get_selected_point_cloud_map, this, + std::placeholders::_1, std::placeholders::_2)); // publish the map metadata rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); pub_metadata_ = node->create_publisher( "output/pointcloud_map_metadata", durable_qos); - pub_metadata_->publish(createMetadata(all_pcd_file_metadata_dict_)); + pub_metadata_->publish(create_metadata(all_pcd_file_metadata_dict_)); } -bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( +bool SelectedMapLoaderModule::on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const { @@ -76,11 +78,11 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( const std::string path = requested_selected_map_iterator->first; // assume that the map ID = map path (for now) - const std::string map_id = path; + const std::string & map_id = path; PCDFileMetadata metadata = requested_selected_map_iterator->second; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -93,7 +95,7 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( } autoware_map_msgs::msg::PointCloudMapCellWithID -SelectedMapLoaderModule::loadPointCloudMapCellWithID( +SelectedMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index f44d549a0f576..eea8b8c1950ae 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp @@ -39,7 +39,7 @@ class SelectedMapLoaderModule public: explicit SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -49,10 +49,10 @@ class SelectedMapLoaderModule rclcpp::Publisher::SharedPtr pub_metadata_; - bool onServiceGetSelectedPointCloudMap( + [[nodiscard]] bool on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 96f9d114ed265..ea2c2e7033014 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -20,7 +20,7 @@ #include #include -std::map loadPCDMetadata(const std::string & pcd_metadata_path) +std::map load_pcd_metadata(const std::string & pcd_metadata_path) { YAML::Node config = YAML::LoadFile(pcd_metadata_path); @@ -33,22 +33,22 @@ std::map loadPCDMetadata(const std::string & pcd_m continue; } - std::string key = node.first.as(); - std::vector values = node.second.as>(); + auto key = node.first.as(); + auto values = node.second.as>(); - PCDFileMetadata fileMetadata; - fileMetadata.min.x = values[0]; - fileMetadata.min.y = values[1]; - fileMetadata.max.x = values[0] + config["x_resolution"].as(); - fileMetadata.max.y = values[1] + config["y_resolution"].as(); + PCDFileMetadata file_metadata; + file_metadata.min.x = static_cast(values[0]); + file_metadata.min.y = static_cast(values[1]); + file_metadata.max.x = static_cast(values[0]) + config["x_resolution"].as(); + file_metadata.max.y = static_cast(values[1]) + config["y_resolution"].as(); - metadata[key] = fileMetadata; + metadata[key] = file_metadata; } return metadata; } -std::map replaceWithAbsolutePath( +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names) { @@ -75,7 +75,7 @@ std::map replaceWithAbsolutePath( return absolute_path_map; } -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) { @@ -92,22 +92,18 @@ bool cylinderAndBoxOverlapExists( const double dy0 = center_y - box_min_point.y; const double dy1 = center_y - box_max_point.y; - if ( - std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || - std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius) { - return true; - } - - return false; + return std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || + std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius; } -bool isGridWithinQueriedArea( +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports cylindrical area double center_x = area.center_x; double center_y = area.center_y; double radius = area.radius; - bool res = cylinderAndBoxOverlapExists(center_x, center_y, radius, metadata.min, metadata.max); + bool res = + cylinder_and_box_overlap_exists(center_x, center_y, radius, metadata.min, metadata.max); return res; } diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 29d9a24d7b0e7..07e8ade5c6f7c 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -37,15 +37,15 @@ struct PCDFileMetadata } }; -std::map loadPCDMetadata(const std::string & pcd_metadata_path); -std::map replaceWithAbsolutePath( +std::map load_pcd_metadata(const std::string & pcd_metadata_path); +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names); -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, - const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); -bool isGridWithinQueriedArea( + const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point); +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); #endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/test/test_cylinder_box_overlap.cpp b/map/map_loader/test/test_cylinder_box_overlap.cpp index 4c9b18a3dbd8e..d8ca2ca9f8734 100644 --- a/map/map_loader/test/test_cylinder_box_overlap.cpp +++ b/map/map_loader/test/test_cylinder_box_overlap.cpp @@ -35,7 +35,7 @@ TEST(CylinderAndBoxOverlapExists, NoOverlap) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_FALSE(result); } @@ -57,7 +57,7 @@ TEST(CylinderAndBoxOverlapExists, Overlap1) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } @@ -79,6 +79,6 @@ TEST(CylinderAndBoxOverlapExists, Overlap2) p_max.y = 1.0; p_max.z = -99.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp index a832489b6db99..fcec100f389c5 100644 --- a/map/map_loader/test/test_load_pcd_metadata.cpp +++ b/map/map_loader/test/test_load_pcd_metadata.cpp @@ -21,7 +21,7 @@ using ::testing::ContainerEq; -std::string createYAMLFile() +std::string create_yaml_file() { std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; @@ -37,14 +37,14 @@ std::string createYAMLFile() TEST(LoadPCDMetadataTest, BasicFunctionality) { - std::string yaml_file_path = createYAMLFile(); + std::string yaml_file_path = create_yaml_file(); std::map expected = { {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, }; - auto result = loadPCDMetadata(yaml_file_path); + auto result = load_pcd_metadata(yaml_file_path); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index 2b686dc0fe8c3..5667f476b4dab 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -26,8 +26,6 @@ #include #include -using std::chrono_literals::operator""ms; - class TestPointcloudMapLoaderModule : public ::testing::Test { protected: @@ -61,6 +59,8 @@ class TestPointcloudMapLoaderModule : public ::testing::Test TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { + using namespace std::literals::chrono_literals; + // Prepare PCD paths std::vector pcd_paths = {temp_pcd_path}; diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp index f61dd188f0679..03d533d41cf18 100644 --- a/map/map_loader/test/test_replace_with_absolute_path.cpp +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -37,7 +37,7 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) }; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } @@ -56,7 +56,7 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) std::map expected = {}; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } From 3c615a6b02d88c3e34fef02bab56fe0df1f304e3 Mon Sep 17 00:00:00 2001 From: Koichi98 <45482193+Koichi98@users.noreply.github.com> Date: Fri, 5 Jul 2024 16:47:51 +0900 Subject: [PATCH 11/33] fix(system_monitor): apply cppcheck-suppress for cstyleCast (#7867) * fix(system_monitor): apply cppcheck-suppress for cstyleCast Signed-off-by: Koichi Imai * fix(system_monitor): apply cppcheck-suppress for cstyleCast Signed-off-by: Koichi Imai * style(pre-commit): autofix --------- Signed-off-by: Koichi Imai Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/system_monitor/reader/hdd_reader/hdd_reader.cpp | 7 +++++-- system/system_monitor/reader/msr_reader/msr_reader.cpp | 1 + .../system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp | 1 + system/system_monitor/src/hdd_monitor/hdd_monitor.cpp | 2 ++ .../test/src/cpu_monitor/test_intel_cpu_monitor.cpp | 1 + .../test/src/hdd_monitor/test_hdd_monitor.cpp | 1 + 6 files changed, 11 insertions(+), 2 deletions(-) diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index f12e34c8aca46..cbb37343be05e 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -198,7 +198,8 @@ int get_ata_identify(int fd, HddInfo * info) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = data; // a pointer to user memory @@ -260,7 +261,8 @@ int get_ata_smart_data(int fd, HddInfo * info, const HddDevice & device) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = &data; // a pointer to user memory @@ -545,6 +547,7 @@ void run(int port) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index 16f8fe9b93477..6de88d9e23276 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -114,6 +114,7 @@ void run(int port, const std::vector & list) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp index e78d49b55a3df..1e6325d9c452b 100644 --- a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp +++ b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp @@ -76,6 +76,7 @@ void CPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & s addr.sin_family = AF_INET; addr.sin_port = htons(msr_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { stat.summary(DiagStatus::ERROR, "connect error"); diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index c57aea81d410d..85a88d2430bce 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -615,6 +615,7 @@ void HddMonitor::updateHddInfoList() addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { connect_diag_.summary(DiagStatus::ERROR, "connect error"); @@ -854,6 +855,7 @@ int HddMonitor::unmountDevice(std::string & device) addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { RCLCPP_ERROR(get_logger(), "socket connect error. %s", strerror(errno)); diff --git a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp index 0f321efe5c311..45ce75cbe3fae 100644 --- a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp +++ b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp @@ -200,6 +200,7 @@ void * msr_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7634); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock); diff --git a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp index 4a948029a2f3a..aa5359ba1ceb6 100644 --- a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp +++ b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp @@ -206,6 +206,7 @@ void * hdd_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7635); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock); From b61db78c8c9d8658bd43789c72cd7dddbc2b76d8 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 5 Jul 2024 16:48:06 +0900 Subject: [PATCH 12/33] feat(tier4_perception_launch): add image segmentation based pointcloud filter (#7225) * feat(tier4_perception_launch): add image segmentation based pointcloud filter Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: detection launch Signed-off-by: badai-nguyen * chore: add maintainer Signed-off-by: badai-nguyen * Revert "chore: add maintainer" This reverts commit 5adfef6e9ca8196d3ba88ad574b2ba35489a5e49. --------- Signed-off-by: badai-nguyen --- .../detection/detection.launch.xml | 2 ++ .../detector/camera_lidar_detector.launch.xml | 13 ++++++++++++- .../launch/perception.launch.xml | 2 ++ 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 373e343ad64fa..b3280fee47364 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -13,6 +13,7 @@ + @@ -163,6 +164,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 4c288aa632182..fba0144e6cee1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -4,6 +4,7 @@ + @@ -95,14 +96,24 @@ + + + + + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 5d8d7563e7a7c..6c428a5adea0b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -80,6 +80,7 @@ + + From cc1ab057006acdb8731a9fd038c24aa553969a3e Mon Sep 17 00:00:00 2001 From: Koichi98 <45482193+Koichi98@users.noreply.github.com> Date: Fri, 5 Jul 2024 16:56:54 +0900 Subject: [PATCH 13/33] fix(bluetooth_monitor): apply cppcheck-suppress for cstyleCast (#7869) Signed-off-by: Koichi Imai --- system/bluetooth_monitor/service/l2ping_service.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index e811ffb030cd6..f20c763d643ce 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -60,6 +60,7 @@ bool L2pingService::initialize() address.sin_family = AF_INET; address.sin_port = htons(port_); address.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(socket_, (struct sockaddr *)&address, sizeof(address)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); From 71fc4c36cb96e934d43cd3961a36b732c8772775 Mon Sep 17 00:00:00 2001 From: SaltUhey <111027815+SaltUhey@users.noreply.github.com> Date: Fri, 5 Jul 2024 17:26:20 +0900 Subject: [PATCH 14/33] feat(ndt_scan_matcher): estimate the covariance of ndt scan matching in real time instead of using a predefined value (#7596) * enable to calculate 2D covariance in real time Signed-off-by: yuhei * enable to display offset_pose Signed-off-by: yuhei * enable to publish estimated ndt_dev Signed-off-by: yuhei * Addition of explanation of 2D real-time covariance estimation Signed-off-by: yuhei * fix name of picture Signed-off-by: yuhei * style(pre-commit): autofix * fix covariance_covariance_estimation.json Signed-off-by: yuhei * fix covariance_covariance_estimation.json Signed-off-by: yuhei * style(pre-commit): autofix * Commonality using estimated_cov_2d and removal of unnecessary publishers Signed-off-by: yuhei * Add a specific explanation for 2D real-time covariance estimation Signed-off-by: yuhei * style(pre-commit): autofix * Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp Signed-off-by: yuhei * Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model Signed-off-by: yuhei * Addition of covariance_estimation_type Signed-off-by: yuhei * style(pre-commit): autofix * Add temperature to parameters Signed-off-by: yuhei * Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance() Signed-off-by: yuhei * style(pre-commit): autofix * Add a temperature description Signed-off-by: yuhei * Add processing when the inverse matrix cannot be calculated in estimate_covariance() Signed-off-by: yuhei * set default value and range of temperature Signed-off-by: yuhei * Simplify the code Signed-off-by: yuhei * style(pre-commit): autofix * Fix explanation of covariance_estimation_type Signed-off-by: yuhei * Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters Signed-off-by: yuhei * style(pre-commit): autofix * Add explanation of 2D real-time covariance estimation Signed-off-by: yuhei --------- Signed-off-by: yuhei Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 12 +- .../config/ndt_scan_matcher.param.yaml | 7 +- .../ndt_scan_matcher/hyper_parameters.hpp | 59 +++++----- .../ndt_scan_matcher_core.hpp | 2 +- .../media/calculation_of_ndt_covariance.png | Bin 0 -> 103464 bytes .../sub/covariance_covariance_estimation.json | 23 +++- .../src/ndt_scan_matcher_core.cpp | 107 +++++++++--------- 7 files changed, 117 insertions(+), 93 deletions(-) create mode 100644 localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 48ce49d2bcaa9..1881deab23d05 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -241,18 +241,26 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Abstract -Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses. -The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. +Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode). +So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE. +LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix. +On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance. +Ideally, the arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. In this implementation, the number of initial positions is fixed to simplify the code. +To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood. The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. [original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/). +drawing + Note that this function may spoil healthy system behavior if it consumes much calculation resources. ### Parameters +There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type. initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. +In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature. {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index f62329b8bdb6d..616cb108baf4a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -107,13 +107,18 @@ # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) covariance_estimation: - enable: false + # Covariance estimation type + # 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE + covariance_estimation_type: 0 # Offset arrangement in covariance estimation [m] # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance + temperature: 0.1 + dynamic_map_loading: # Dynamic map loading distance diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index e488b49393d48..d6bd975c36bf3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -29,6 +29,13 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; +enum class CovarianceEstimationType { + FIXED_VALUE = 0, + LAPLACE_APPROXIMATION = 1, + MULTI_NDT = 2, + MULTI_NDT_SCORE = 3, +}; + struct HyperParameters { struct Frame @@ -80,8 +87,10 @@ struct HyperParameters struct CovarianceEstimation { - bool enable{}; - std::vector initial_pose_offset_model{}; + CovarianceEstimationType covariance_estimation_type{}; + std::vector initial_pose_offset_model_x{}; + std::vector initial_pose_offset_model_y{}; + double temperature{}; } covariance_estimation{}; } covariance{}; @@ -148,33 +157,27 @@ struct HyperParameters for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { covariance.output_pose_covariance[i] = output_pose_covariance[i]; } - covariance.covariance_estimation.enable = - node->declare_parameter("covariance.covariance_estimation.enable"); - if (covariance.covariance_estimation.enable) { - std::vector initial_pose_offset_model_x = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_y"); - - if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - const size_t size = initial_pose_offset_model_x.size(); - covariance.covariance_estimation.initial_pose_offset_model.resize(size); - for (size_t i = 0; i < size; i++) { - covariance.covariance_estimation.initial_pose_offset_model[i].x() = - initial_pose_offset_model_x[i]; - covariance.covariance_estimation.initial_pose_offset_model[i].y() = - initial_pose_offset_model_y[i]; - } - } else { - std::stringstream message; - message << "Invalid initial pose offset model parameters." - << "Please make sure that the number of elements in " - << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; - throw std::runtime_error(message.str()); - } + const int64_t covariance_estimation_type_tmp = node->declare_parameter( + "covariance.covariance_estimation.covariance_estimation_type"); + covariance.covariance_estimation.covariance_estimation_type = + static_cast(covariance_estimation_type_tmp); + covariance.covariance_estimation.initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + covariance.covariance_estimation.initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + if ( + covariance.covariance_estimation.initial_pose_offset_model_x.size() != + covariance.covariance_estimation.initial_pose_offset_model_y.size()) { + std::stringstream message; + message << "Invalid initial pose offset model parameters." + << "Please make sure that the number of elements in " + << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; + throw std::runtime_error(message.str()); } + covariance.covariance_estimation.temperature = + node->declare_parameter("covariance.covariance_estimation.temperature"); dynamic_map_loading.update_distance = node->declare_parameter("dynamic_map_loading.update_distance"); diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 2f3eb82b1c217..119c3534cab16 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -131,7 +131,7 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); - std::array estimate_covariance( + Eigen::Matrix2d estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png new file mode 100644 index 0000000000000000000000000000000000000000..f6e25d04c0fd6f9dff9b924876c66e2ac2df17d4 GIT binary patch literal 103464 zcmZ6yWmsInvNb%xA-D$*1b2705G**s-Q8{Q;1WV`cemi~65QQ&aCiGA>GDkq7AfQJA80Fb0UizxyCFtPvu#1oeE7#rI-n%O#?LbeNlA7V9ER(BG0FxGc6 zx3wWrHn%ng-vj_ym{{0$8f%z1c2h%`S$7liNSK*-ll)mYcS3@gnfD5QPFNM=0RSWb zDKTMXx74E*pqZvxI_KHx(SctI?CHr&`TI$^!3Pt?5Eg2&kF5AfH_?Sg9S9#nWMD{R zMM{iFa%!Y$ImhSsiNBG5R~{9zFeRzg0Fm5gi6gVA`(e`hHiIg01C1H8i4=bw;h@A|Mg=uu)0J3YXF$cME?HxN{0EgkNLue(S_^AVB01< zu-ZK0^&Izxr#}01Od=eS75A~*a{j@A^Fbh4B>Y#6#IPs4m;hIV&MmSP(4E$$r3oik zFhq~$HrG<(O&IVz+$ZBmz+z@o_KIza3Q9~;eATknL1>DWL|d-KzRq?JgPBYq3-Ll1 zzDhxiAO-Y>jz?m`)Xh+`#XQ;&#Dt@XwS5uFfCNCNhx1TG&EuI^KQ-dq%=PO-`1Xc= z_0UhRaY2*;ZcDB;BBsJVj9ZV9T^mCgat>t`%i;Lr^rNk=-D$kdeBz<{e^fXTo7?g1 zjcJoO02GWGM-!B=NuHS_@NBWGOKfM4B)HO~&OQAAz+CqYU1!*_SAL=-ccqRPKoY7- zWYUDLw~<66n#!UQ>a>a}PtO*5HzKoGUwb`yMm z7Al11Y~`l1u?~LVYYsIoe!$tWk|iUR9F!oj@Zd~v)gxf{_rL|m+si!+{uxO&jdKo6 zGxT^utx5vIoM$)glL>6LpI1Um+#~KD7RPsQ zz9C@dHSJ^bn>)hGXwk0Whap%7d>Z5+?#g&1vaEgk2WD06?vq$=rn47dU5{fExtCPR z>wUbx;7Ix1$=LbF?~PE^?{AN4Y)LZwoskB<<`bN~W|s4ui+Y&Pz92u*_K%7ut8abm zZ`#i8kD`6kznOCcfrE5L+`{8swv)3XWk!wR&f_Xlp!?cx&5aOzCf+8?2I zTRNYdVwLz6RWu3YhcrYMS4XCJ=cr*GD!lR2?f4@~$2wu%(VfgSYkR^J6B80w&Vb?8 zdw0CsgmOvt@YJZ`V=SGmoy3x6)gk&qb>I8k3dT2Mto<&G@g_0WGlrmu5DYDrsf14jQqbL;$hHlgNPh!CuABO7V^ z+PlN|E>@FA^H)n~x%jd=_~sGMp2Rwyf5MO$t-2`sRjoWG!jNWVLEb=ALrzUtepMA0 z-Z1dnCZkt%7xZfvtNv7BY3ly&wpa-~nUgnsI@#h6?rBL7+71wKvUIUJfd9B)=FonE zr%@{qkQT0-(Mymoa41<}sZ zV`9a$9G%V9j3W z5Ik;iO;*PZv*4b)+ug-Fxr+zEKVOh^CF~_wY4JwR>6`?;8X%8iFc0%lWu7sGPrJdj zMShcczGr*T;;p>RS^VUZN4Q_1s^+5U0FQX$(gEav>|avIz4|b`oY--ufMb;(RV0X# zJ+~wA8n-;@UKR%bJP7yT(~NbKpwqqKL)<<6+|1njf_RC2#bFFMrAe#f|e?i>8o?NRTKiQxHZD3Rc0J16sT7Z22iA7Y!S1=Bdy zO1Kx-`k+ykOI;gh8QNt@QQYMMcG(dcE$s<#-;m=90b#CvZp_ZP&! zZewCf^v&jJY!E}bA>NVrl12gF!4p`YacyFp+5{J3!_je^>srU{*95vy&+yfbFu;4I zF}ADzNa`sJ9FrhVDr?O-tCd&&b*MkpGl3DmFLK}O^~GSXo8{@cMmhawa4h<=;+Io5 z#)TS+??vzIX?DD@3J_(munWG~ELc+yY-=WWWJ|4E#JUQ6QC$KESp^&XwEbbCCtdcE zr8y#<%9XA9xa`Qh#J6#sX%=U>iuL1XI8V+k1JK%fP$u?YYTeTwP6Q= zw`C*{j|4Ner-vDc@v7|V1ZOX!Gc^>0<%XbB>ICb2<}2a*;hzs|Lv+pbw`5$;Nyt`L z9nH;uc!BE>77(e&890=FZfPn5#3vhtFEc z9ltqw9vLD!865zaT&+56+QB&KJ zhY>EN*lNQ77X1XAG7?3)pnl*>UMV8Ck=4bYzU9Z+1^E9`;~Exhta8^b+?ETCDH5Tr z+uB%@4~JYf@+3OkSItw%&Y|_i6)uLsEUL*M4%W|FWBPiUD`K|}gErxz+seh5)6{qa z&zX!|fo-Gl0#G!<#Nh~%{dj75G3+m5nKxHmIZ<(H+KB^3PJq$LaD4u=Bjq1=vrrLi zq_!s=Jq19JAhWNK{LV5a^yqW?eG)e7=M}2k#csMQ;$&jg>}x30!l!I5ee2}keg^30 zwltPXN0sfBVjh~D%bLDDvH+%-miR#Tp5_*n&u>2XN?CvW&Bj%QMQ`PnT5N9xZ6tWK zc>|Xw`-bRRMW8ByupsGF|P=H0$`gQl;ZJlLIlYC%x4oH8jY&a6oyxc&?O^lwX{(nDHI~ zy~xdF9Q3+W4EIqpV1#aJH_B$_4>E{2cjs?@6nWic^KiY@KhoJ+j&$;PR^^yUGlevYu_O~PJT7b%ttx&$ zYVgn4Op*S_*ymbqqwQz-3dlAmEE)^2bDb&UZR8E=Xhu`^ES}QgA6{ws zP?Dx!9k_ffb9rfqw@}!r1Mh)}B(rAYu82 zL3aNmbDLdCae>$&(g>oF=@61~D#&G~>{C*tAt?6yoaFb*ueh=MpMp@P>yB8;BlNS; z*%b)poARE*xWC6#pXUX9?Bjx2iz2N~J^pRAVrO0cWIzh!_sbK}a?feuPZj^}DSyj? zK=w~uwEq#;B!c!CZp9(X_0f>7X6XHCNnDW&hSJtn8Rg)f6KF>-aB(Vlwcn1W*)h7= zCa(we$rc*4G=nyeJKl9)Q+aEo;FksFJ3-4@gr1)1B-Ecv%XSQ=&2-@yt?iDgkes`<%sm<~ zlo$R(Lf?N9U-$hhu4gcDhJzUQG#?u6pGg9)$Nar$Fv39TB_DNjBkyc%w)obn-j;ToJl50fl zv+E0(qVyS%0kE=La74{3B%jyTTkcVVz<9BR9c5 zd@QA5z3ph`?J$H?+eke-wE*gCiHwnFPCag6)?;8lF4h{b8IRkNzBczqIG^DV8mla>UEun#?L zGMwpmj0YiJn0`7?R*%6-!y$LrCl)xNH?0-l=a1d!w13F3V_sCj4($gVk^rBjNLVv81LJrO3 zqKm5u<64vFC|?RlyyKX?P`(W`ZqoM@&mU_j2Ev3>ytoB~Us6#B)Rw1*iF#$ok1;kg zkyX{FW&;eC2raxKul4N{mMYrNf6b#@TIzld?p=ZR8zJki$D84x#n@rdxXyCXt12SZ-@X)3x*P0PD~@dDo9 zEtG*M%Sd8Py*5|&&?QX|LqU1OVGX0B)4$u8>Dqt{6Vou}yo8&p#6LhN*Uo>w9Pw8) z)Gy)5gwbXnUxb_v8yy1ra!O^=uS2V=Zv>pHl~Q>L4U|X*O`@T2Cr)9r;Ov)hlmA0(5{1f0zvw3KI<{$!2Mve)ja#ql0A(kmDW0j&TP7gi zGbSb$U(NCcg@B4ocYn0S3tdDZ8zO(@VdqL^XmB+j64){Tg3+!Fp-l;xCWYb*$9nve z5qf!bcZaYokY^T-Aq=25z{~|g!01QAPoanOcT?G2vpYh!M4SX=$$Gm#8Y?=gzCj}r z?iaBsx;k$oVuTfUo)_bv9M|2HD@+JIq;M&^I&I59TO?qd@Y`_lBzPZQB#yUB>(Qk7 z30Oi9($vte*qyp`-8dmqyrt+GqO>T<6sNEEL5>_@~av&c0r{QmW3G^1o9)y7O! zZ0K2J+G#o7BSwqyIPaX5Rm8~_lL{&oH?ECM}RtR?#UcJ zw7_5ayVq?oKBvo*xJ*~MNdXIBv&Q)ILO10f?C}-zMz1NNVxLPPsc5+4bw7~SVl)h!% z8OGfsh-mN+g%kUFJL_8yUF!X+KQV3nd!##um$yG>Fomd2AEC6-f~*!(SU2NStdIth$JV( zjoYWQlgkcACf{*&grYH^owCW?-Xv}ts_tgWB?nj2!EC5{X(jvP_)J!(vl(Eb2=S+B z>-gXf?wX|CoeJkeJ9Bc&Ik#R_rz1;PXbE#5v0(>PFJcff6q*lSykH?-&E7Mldjd?P zFgn;`J(5ReDqh(2>#BDc{>j5ySe7Yu63#@SrO-3xp=Zg|SV9;<;#KGpA8Jnv2@K`q zbh1*H<9`>9RDNsl+54md+AM+^@!o^k$4nwX6M5*h|z{_M$CT6Hxj61+wr z^SEXfKO=H8gM8HX)Qy|$f4RPv1+AIE;fJofZ?562VwerL&6e-t+LiRQ!x!0$hx`T31>R;EUOGUp7hTVUS+&-5=meXA$EI&&0Us|2FRRL|Nb%&3B^ zk!9szEY0(1|DieN%|guL7E_{5YO*Hm$*zClSXDv{2>ThJPOebzncSa;3) z9D1*f`M=sMU0K|K%;OdvjD4b9m6QGM_3w{irrO)5Z0_WhPiIySdW01#-Lgp*?$( zb0>{gh%gI3D3UN!FbmoX(?&hR+Cadx~+QdoI(A~aXy1nAZ_`c%jC&9~of)Fzb9 zpLJ1KIau}P*C}iYS$rBt3Mp3G!dO@PHLI{?Fu` zFa?8SIF6s6&Y7BYmjQ?2DV0*yT_>R4eOrg{>V}Mt9~Jl~81UQ0o+<@sH`m#>t~>q2 zqysfmCw=sO-uGo2HF;B5T7 z%OuCS?6V}c+1;k0NsFQ@*oUJ`|7@S*R1l+nz%=~`kNB+5Sl3qhPSu*$g^RQE#UCrh z+YvFw$=iC-#t8Mbfw=XrFTMTfQ1SGn1+ztbWzZkK(>7OH*;a$|oPceW&zvH$z0x|b z&lvro8B4Wa2T^A-0q*zqkGnf?v*~zVuhy`{5w?y8jL>$$sOug`tif&#upMVOH@*I! zCsP%f6``Ozk&|t~Daae&)% zIlC8bMu&$GVXehKv>Xy)t7uVDlCRMmihW+j$5P;EBEQ?u1>?M*H}qA(#oxI#?Ouu5 zCU=U}4|!pe&2IGNJ=7URHJiC70UA&&%Tu~qYF(3uhSg>}4e5f^hk0vDEuR_k4b4mG z)E71?zZ@_$cO8}^OA|Y8=S*o$}XQq4jNI=-h1hopm*1{ zRf1oYVZrA@L~g7n-6$8PBvOw@PV;Cl;@_++989qqs~~mYPK|lN!ip8=2TZ2gvESrk zqnV0C7y?hvlR<$NnAh+r^;>G`4_T=zdQ)yeILv4r=kQ5frzJCM1{kk<80gF97<*Yi#4x6Qj+hwlJ`~>7 z&rxK}1WfeBjz$}7mdLcYx{=Sm>{uP?e-UX`@$02n4G$M(_}-vv6~EZyb>Sd1N4QHry*aNA#oi2k-{GrqEIeMfQ%43R3I(yt}d%4t;mx6avB z3E2L;GEsM*(%rdu22-T7Mykrw)qzDig1&9}qnu}|94{EBAy`!s;+UY6YAc3QDn|AB zprxLp69tX~*63oxzm!PsOC?|D8x-8rHSNn4r$M5pS1jeU=~2m%wLFs>O_XP(r)gaq zxwsSG;T8Z82kN8=B$+91C&S1hu-r$9VhT;CZ}gQY6Genc7F`$J$OQ;kf>WS``K(E( z_KPnK_hE@ofp^Uo7*{9N;oX1>D(cP=YbaSF*^qX}&`c=~inhodm9M6Fzf?GYeYM4L z6e{C6V;~ppj za2p{zHhphh%XhC=oe`h;EgA>df&@Z@Y=2cw>A21VF|c`$ZoU{}HOPxIx9DpAl3Caf zYS#L+>o8Ed!Ay#@LfE-Z`62P^hQM@iFFVBee$d{`mpn|fWNTw%AWq*p4mSWHRmXPg zH$doGZ`~T_ORF)7JwssCLL3%6z|7rR=F{kv;gNtZGUt;mS8~47(bD^V#kod8LXLS} z8t18XL7bra65%Fmn+8rb@2c!uD@4eO!tkjDEj!*J!;(T#Tei=%*Q%I>NQ-)NY|0X* zt7c9vLyIf_t-D(P%uu9sR%V&iM2Q<=>>|eQv(w^?BaWDtf#~u2Lm#c`#xp%9@y;OQ zJw-v@lI%y{dZ|p2?~iV%Cxf|ch@Z;yEEsVLm>lejoj%uYc0|#a4c2rzb_5Kv)w=Wh zS1m;r&&qjXQpG)JV5(coLZD#tTH(_=k92DttY4@MdZG1B699~1o3xQAw%-BpAao0U zJn@9h&tMwZvLm_H=#hFe`^%vDiuXnxmtBq{Uu88r{Bhh0(a|UXO&oDayl927P%W+4 z)rY}a*L1dF3u9RMDiUTOVE18`&~cdEG~v*XpfBnl)4i&*{m9U?lcyy@Onu+Av?YGF zOtOshTj5|y#p=qo3-7;mVyOo$9Lp1iGzuvkp(Ag2k=uMDduad@^Zwjf*ZOBh+UtmO z-c*#y)TL#13674M_0PG6*rGDjMt;A$m5Z@7m%@?Vtf6VO=Bk{ZeZwU7t)1JzW{myg z+-j=%ubkmJ>bQ0qO5C5yO$T=t#~t2%4Ouq7HJC3uipCv~LpX~nFNxoFll!EXrOGkd5n6rG#%$wLFIgTdB8 zY)x$AeLdgacakfs^7(B77^g)=8ccn~#fN+h8R#a+q4EzChc$-%S^A)4;%>@i%lVt# zTjIC8`|o8!Lp>!E9?$`^7HU{X;0GWJU-qzhr!q|9o@3)2)Ei}^cIikq8}=;jX^q}s z)5VceqIo?zx&IrhKNWB|?z5 zBUBS>#C9ZnAWCC$s`PJuvI2k@I>@>NOR)Z1Zv+|p!vt&hz)YDuTzS`Mv6+nT=&GNG#{U}mM6ss4M)o@ zA>PGmct!O+w3o@8B{V5(*)B|1(LXKbt?e2nafUssMchnurX^4|i@Me&zr0h|KZUX* z7E+MhOkt~BYTRzVvfX5I^;}mUbzUy)v=QDXxy$z3U*Z{`dSG&^X2{O6vDYK-AFI#r zSX8(@M9HJ?NH3s81TeAUUU-cAi~#DO(VqDLru~z)nsF3hbx$NW6Lway+8Zr}dboO^ zHm&QfzUs2q zl|bu9gLy3*t~v9IjwgY@moW=U1*)Y0IKpo^ zMW^I#D&LlZq=oA(C7t5}E=p{i)YQ~KpLH}1+{Gt^R;k8%oSeF=AGvw1yEc8db{4~Y zhjGY47_TEa$)cu+%htcTXmV<$(6%Rj)|Drjqv7{$QD_Uo#ya6gqWFH$XWe|A)DE41 zVC}fFfbWX_dD)gV#}uU5+QIN4|w+o#gnhqbP_^8>DJ*<2GtdtFQE!K}u zGe>U$vBt|1pN%>{=1YpNxab8uElb73P>~^cNQ@-S zq)D-+gCJMk70!mHmb12EM>fSyPg7^AOmy%?OO1U+g1sxWVk3cPr{B2jrWa{qvi(F# z^yI7li~&-cn=Lh!N669c=~jvro$vX0O_y@ZDT|l%tjtV38_D~pXux)#eLcDFdpnH! zr0qOb1n#$g2s*_=I$#y#F6wf0z}!&M7aO}OTe=AoI~0DSOA+&Vw!rB`kW}tS&6xoc z8?_D@FRZQ%ucqF)-MX#we&{^=_!9X~J9w6OY|IQuWDYo$gm5`Np>Y_r+yqN+uy(eI z5QgFB+G^&e-AM^f(;WajHFHvGns2MA8yx$(+zJ(1`smc>!vZQwD&rYzw=t)4)D`?dUS@*UM9gf+%9odH7C0xheo61EX{<+q~ysX4q`KQt$ku@ z$zS8(jA_9PShaJQ7kk)Z>T?9tL7@s%xsgZy<_+A)*+H@DCFqN~O_*d<{(rj+vC(cp zeqMPDj`tL?v+l|+DIybK79R(*?QE+_m)ks5453CI@5raVS1n|RZFGkQH?Fe7V|6U z$^fKQRyv#kf!i``mYST%!|C5ckQrYwV?g6#<*GbIIeKmQq@#`O+Q)pS$thyjz4FbS zC7`Ev@;k@gAkfM_>ouZ%*4q#v;PGbjsumj3zy(3nHoY8(%0R)aQ0~cI;exdUq6G+L zam0tYv*EKuBG**xxixZd$2?k~`i)te~IvXvZ-Z|7sr_E^o1Hj90IA!P;6qboIA^8 z<8+~Wb(6`ZD9V;NHTL?rh?pQqCrqWcMBGeYz$)9fVZ|m%URuz-bu5I&cj7iQ=@A%Z zhCuIRe|-VgKN??2wB0-eeVmGb8cHJT;2ISIvqFjw`<{4#zsP2%xW!@jWxr{#EI~y> zZ>=^)Z+6(>v(z6PiXOq!wy!;Pz0|9$vChxBKc>0Io^CXi?T^l-&Ys`F6P<@lnPAK* zytY_EieoKdC)_dQ#?LI>m-x8@cLbKUla0GB7x&yOe{Uql5a(=`S>{;378)H^D7m~G z`jQ@)6mZqgi|vntYkTU2is$h1cI#KOm1~_XaEfN8y-nWKeqtM=*2Qv}TT(;h4LA29 zxJdBJD7;-E1jJ00`>9464NR-)6ZCm2#)(q$0w{Jub!C$T&$)fj=NYrk_bFl*j8Q0z z)_OD@WQR=dpr2;> zLPg#6R8OfQI2@t;30#$kkN(Z#K1(?|3LX3Bf7>pCwyU~DmjdS}_90D7^W#~eCH<6; z=Pd(e3h)GW;oWtWrWa}~CT)mew(Xh(X-Jc;n`moA&=|Y-xq%)Q!o>4h zo9&4fd_}AA{=*R{AqCRkydrtiHhB}QQg0*m&yO8L^Eu4dJgZ_H`j5f^s(MHY`CP3+bv>+w_ooQ)N{fi%i>A_iGl3mf+IadtuoX#+&xN4P z0OIRL^d55)W$+az{3qK(rZ5EMlpw-pemiG+M^O^i;Hz4wpRP{0j=qycof}S_t-lPv8&4FWBemXU6WaB9ArhLyr&Jh;Nk+Si&VOZ9BVj6O~sfsJD_xo1) zcODYK#8c*XM<)TU3CBgm_~DpHM&*<-NsvbxoLWoRPXT&HKN1HG0&!ybivHD zTbpyL=pH_<)|J>s6j;}nmsrhhWO~U&k8s3+bincw&1Q;}sg(BWGoXd+QSOVxfmG*m z(Zm~glJLlE3^7k2#)1)K`RBL^4DVJCj3)USe6O3R`%T{#MiHTfy};dJQQckVFDNja ziVU0$%wE)AD=hDREhsTBfj7kd%DXo=m^u@~&e5@G9I$AkBMuo<#DGhj*R-Rrt-1sT zm=n{Yn$GpR8WO%QEc7LU*(xhTq`Jy_EAzX!nvCL*584&WkoKl-OI|ChzU0l(r_s3D7{A76mgO1u^9pHVa z@GY$%#JWJuKci0l_}jo22f9x$E6G%ZM4_tK_jx&g!OC96`in|RCZFbUiTJ9?TsvAC z$Y8DPqA%voc00f@*;M{_ALMElVXG-Jq6RxY-ukQj2%E3-B)@aVr`5is4sAQdnDfU< z8VZ#$+jPHB9VE3bu*C9Eby zfgiQtZ&Z+t?$J%J0V~GOq5vfK1gb-L)+`>_^iqt9cK;k6^LZVf+j$Y8Pdkj|_gGI=|~F zKDFqU)UDW4EnY2GKPjv8;bT=D!w-UHehq_Ie>U;ZbT(N_#94cXzKmz%NvnU;eE+jm zKqHy1q8a-ymC{}L$2RrtCcQeXImhGf+^vGDqHa)6yJQJ;r?298E)#wCqg7-DJ4s!7 zWiHX|%okfGo6ImlogZF&m2S&cC{o33r|+lGf!)-_;#hlGl|7T+s4)?KzUnJVtQDov z7Mvh{ma%o}G%>m;b?uCA4{I2spa0E~@zYOxhsVX;NRN;F-W?u+1%o(#y$xD(fdMfY z6a1{m*A`yuO9<4wOex`?rbBz9Dn!@jF2TkRF(IE-Db`%sTXhc7HyoO3@pwt6GGNH< z*hu_x(iguuW6SbMLhS58qH}#)^kDa)rP#=^v7d$|q?d-0RkbI4|B#EwNFW-QDdqC} zWUUMu1&r8}Lvp`m`tm3+vRvlUS5W6KWZ9;jJYYByjB2UO zlXVNu>LkB-C}(dez!Migax`tmOcd##JJA>{?ZkEb`4PB!8D4Ob$_Vh6t4B0Vo-Rtu z{DacH$sj+z7$rmZaO3kLt?N9gvhX4K)%s3@CW$U%CbJbSR$M<7R^eQg(hti%O*>V3 zL*nyt8tSoLPUaWYY}nex>qTyoKXIwBRhxCA1KR}(p+IgrKkb7SdsgmU3|WQV#xX|7 z0udIwX-!ucowBx~FZv6)VuF!UM^l*%hoe=FEu0@QY`Mi02shoys2~eqKG}`ef|iB) zCl(i+ziVQkKYNH!j6U`#$QFdXZWi9vHIlnA(`<;2TKEd>e<*L}V4V?gCP*@?geWAL z!r`lZncqxMkG<0Z@(7{b-b1@8eWdQRMzy*cH0!R?_%ZFKOtD$j1klXM{8J<2MHCxa z5!_qf$S{5`p-eH>(7S-~YeMY4r6-t`#XY}V2V2EUS8k|>SFXzXRthwfJG}i~(Je7< zTil1f!>B6Mwnf;^j{YQ8lKEWa70JviU&@GMfTS01f3gQTgAbk4D^|3SUxJNeLmHd+#F;v!L%!{JrNIjVFwu33!o&y{Nw+qe62=7Aqm ze%QIV4V*e|6jMCqwb=Tzi@A0$q1nDe?lG>KAcrwGVb*P3{L}fq$d(Q{th9IDoYG8D zB_y;0-OLJ&?OXNX9Thp`lEs$Fv4gap*LfoSv~Gr6hRhk0=ig?khr|<-_XPqIk*YUG zOa^O)k#UH(fg89M5mu(-tPA=fGZ`IW2EZh;M+b!){9D|WGOto7hp(}8v;Cz^1TH(wdpU<2ah5-PAGdF@6 zm%?Ni&y|w@qEUMZ`Z7>9S;QaWh)Qv5c^hW>8>n8-a=Cv?c2Y zl4iPjcK@N37hkPu5fqH|=mWmG+-A=OQCOW3qjV@R@Vef!iL9|b_^iO3-s|6SCisj* z8lJ5%Zvzt7zOM_W2;UY_byd{Zd$|>~&dy>hH@7aj4yFr>tvZNkry``M6V^WVqlsPl zqi6(JG`P5`4Y8l@4=~77!Wf!NmvPX& zc`pqemS^+3c3$sd!GCeP^e?NMYuqmRn|=0cLp`=$k~FD3(73v0G3#UC>RE@Q8EO_; z6bv;-rfMj}F!XDF%3ta@B)EN1P8fOkzQdg*bYBc{;{4N39H2ACGR$VeCR3O%OG?iG zaWK>L8CnL86O(NobL;S{uIiF1pyRcu_Upo?pRH*uw(X7K`q!1*%VQ|?TWpHrR<)&- z_pY&04ojWF1WT%q+U@B=9>Oae%GHWhx4!klfULed8h`ah2IPVqV~wMu@AZl<3X)f2 z_H0i)fd|5OA}c*ldBRYF_@b{dAB{XEP=N&sKAhuB>kacTdF5+ZdHW4Y)*8nG=5e?0 zIh42LP$I>xwPLHP=@5RA8^~CC5@J!!_;~(pR**7*n-w$bwnV#Hhdk(iRm@MncxN+v zN&Q^V4X6F(BY#|wP4*&2W5ttQesI;6F(<^H=o#BHHg;K@&ttt$_n6O%qt#*!eEVH5 z<5{@w6NJy}t$pubM~p!D zhRR65u4{Qqq%lAVrU}_?TvmVRX%8W}FvmRSbsl3(L>%=>3DgnWPdUKXlwG13ck!$O zH6QjODsm1!UogFY4&J53tW5_%%p9fGN}afr{Vp5I>1bVpJeDWRd!d#iyM**D$1*au z?>clgI5TOwdR#$dp;$jMi*Hjgf!S0;XBu>_aaq(PhoO^e(4Db+$k*|>y@vWJ&bW*; z`1LlB`AHSR^EJ`$+cfiy3325@bm;pw7~!6{=~fBzTa0MvS|O<%A!GKHAdBH{x_F|? zgA4V3xjeZt9j)NQdASUYUk)9`(+SYt%t3z>u5>iPh0M@P5S4*sh7&!g!snTmh|kRz z7KLbicO+$I&zeJ%-B%Gl|LJ(`s+5rZjNr;CD$}_)6MP31n+o?oA+!nz>%q`Ii8R;f|fC7|*?p2OY zJATfiX2N-?o0Xxd)S(yQ8(S`&6)|}nV{Ty~baYhi>9GC9^19LU_C(*rBp*-ge(m*H zce&LQ$MtlzEPo#aQm?_=A%58)P8Y)fTSWXw3!&d@2Xn-V%EpTxL}={EO+C2=2lSWV zutBya73~`n6_p1y@PWcdwA>A`^&#bM$jp?Pteb=azhP^_`Sy#<;21eF$lInMS=H6q zxJr?-QkW{L%3@kgV+a7~>lfH@oOeRsow1lhkY+4RNK>>uc4D)C)s?fn>-a6U;HU(b zK^SaME!>z9@NF8+of8mEi*U-?Ccd{t$SbWmcldU2<0=(=ZX^ARwzf9t;J|8xtd0$qB z6P-Z4(Ys3*fK4|dx=)OP$p!^=tXJsZB6M#!!afzm*%Zu)z?S&YRIVkDq_fU*GDF{J;|dW z;v~ow(CaMJN#w_~8yzQ~#)xX!be3qJBRXUL3bMD*a~Uj*;Xy$a=KpuPci?${1KPN_ zxU5?Ida!Q&`NxQTrE7*t@b!KyC zSM!YzGwwHDMa0iinD{uL)8$1%njd1Ae3$b`o-!Jl&ii7u)2c}24Z!Ll{EvD*7f)+UkKnagqQD*KW?Phvvva7rbScuB60K%>VZ@BEoX2CR2m|Evt$wD9 z11N|Rcp`rILuQxp-S&5~Wn<27%>)mf=aJZQz=Xy_g3mop!-@?7w*wTh_uWT<=PM;d zWaQ2D&Vak?3ZF=yhXS9j#W2>K>+B+o~APxai~aL?TcUaxguzZ=-A zWXObufVhR!m`E}r2`xQ4yFS%;@wqjya^PTyn9K!w={xIiY$uT@8fSx1q z)1w7OZC|bM)r&WxM}V2g$rr(vcKMF;$2NCOw`ZT56Mlwbp4Y9rrIwiFm@rbEPU-CU z7@VIP7#g%v+ErFCIp`}EL(2@xbt#XUOT)F3b!@JjC1se75|_VeR8{rN-TK*We<8Su zVq(xSx-DHkb{iY%2?hGGvbLm0_q&6*9oyDUJ$DIOQpdc;;PKft6uz(h5X>n0F29`h z{}J}qVO6$E`|vUlOjJN5Ejp!CLQoWS(G8L!AtBw3DDX(iqLGr2F6jmpke2T52I>CI zy3;A_0-ujmZuRF=p<#p7 z13$`Q#Xoe_%?THC*Gj35bbV}N7rZ)bxjpLXDktdl^inQR?QGadYn}@}XqHf)QrYwp zl@y#f^6lLrXLc~Jsg7XLX0Yw7?$6x8r_7{PZMs`8(Y1|RYw&X7QWMF%1C2ZXVL8o} z?P0~2AqM*dG7UFHj_h^3yoQNoB%2QyzI#Up3PeSd$yBhNtLG9{m)gup@WcZukX+4jWw*~I8BFpbW1 z<%c^vJF_dJzl6oac#@G7-|d(u4M*%sG7oR4U2E!Y78jf*pf7W8hot*?dcj`-jQ+Nxdf4ogW+a4g}g zc*QpJ+^Mcy$s@%;qurG&{NEoNYT_9E*6gjSWGV2p$87+DHEozvaSbo!)W3 zI5oq)y64XtH0G|(?Nvkv?X%VztgCkgs@<+{QHUCkXkxT{P&2#FSG#;sJdkZXF=j;i z{Dj5R*~u@i?IF8=S`^e;y9cb6a%D|Mab>@xxaAyeg-uh<7fHzkP7%^c?9h^b2(%Vt zDO`IrRk!LN)zUrdD$U=zh9jiLD0#@5VSNT{h|l*_WlxE9rhi1l`_io;`x-CSa^2rG zHNlEbd;PgyT`-ci1%K$u%d$PZPeN@i7jKz%_^b2v~lCJ!-P7Y@;5p5m0pq%C}`bG zcE+IUYcIbPxC)1PQNcGI;w9j8PjTc!J(WB1FuGw$8uV~gAIyD!O zxsjZ&s~&RxC-OqwdAcQcdu*7oYMZB^BknDg4|8yJ^>w~|N6p$IGjc8BXbe<*<6ngLiZ_m$-6Ysm ztOIw-uMMXCDH>q>=DimCYMAjlSOL$67whU6Z*O%LM>P@=HTKx>I>|SIY^FY`SMW*D zEh_t>2S#7tY6V)UG$)XrSM`kAs;bEft8O+>i2mGQ-gV#GoTP^L%)S@-$JQ5^hBbzk zS9W~cZB9+GKGs%6eVIK)QnZb)6wQm*yBYV*&HA?O-uv_Y1Lr*+wv%)q-M(TVP_8U47^S!*Kt=K&|-1M6!1+QCz${s7i|b za%1n$6gtm`Z@k|oW(-wX9TR8_l^MIcsCRF1`JVURyTp`owZnKmk#FN~sNoH-V6Mq; z7dt1hjV|I`xfuI8i<3+JiTtY*Xq&EPtaK?oaViPL&$Ry}7q5f%f$pA!&~#&)l>0 zen699<~X{sKDFh;R)7<{U%G$Vr`)f?I6eroh2gtrICV45I7YpWpA?6z)%g{}!A}R< z!tt^j^x@kZq?)4S{oQ!$YYzo+z3;ye;IFy)n6DmhBV?4gr~BqwCT9q7)I}oZt7ios zwc6;t$r_!$_5S(gTZZ1Y4!LY^-z1GL)BZa8eyt`@jzyT2Y>4f}r6q&2B@FUh7rA2J z*4#8#xpAI_yUfzp|NiZ5>8F)PZ`p3=mK{~EO;*v@bnmHh^5s_EG!Lj~5B>1)GROD# zCPU1|)gmv}#aNoptMX;4E19hl2!DO>bA!8|u4jhV4~8eZzNoseNb!B-g?J{zJI$0fC%xpad+rQbjW6{H>E3Dnz&u=#)hVC2Wy5B? z+VV|wdM$6KVNzJ6MOOSn^nSkwz5Wx~x|UQ|h7C0Zfy6I=lI!_ccLpB_@yb&tX?Kw_ zA3@+Tzc4XrPWjQzg=oE@f5Bhh8KQIs8%Zry!bTcV_ zD~luSWX!sb@5CCtzph1{*_+%L;Xw39;seDm!fUhTEdvUL^V3w$TRMx4TMD{cZ+1#n zN6g4i!5((gg`kd2eEI~}+fd^SOFb3*LwaOH)A*#MD-%vTvuwVXQZ!3f<^09sX@ArXm zFa7lAyVCoLEmPxE*QS~xb>b{${yw#w@8X(OMZS#uPytXVccv;xeEat8!sW}B?|H2j zLfhX5KDOVS`Qv|&$86%iuC&+VJ*_|&5ThHEq3PX2%7yEi0`RqOFhYt&W8>zo?gX@1zLPnec9q*2VRQ#$0#x9~%iD84kM)mfyg`EqifL&wgh ziqi)TPrQqA9UQl25?%>o-zPW1LMVn}wOSe|x}GEDOU^C*w!*~)1=G57=T3@xMfv&9 zh#|sHr8%6WgQ%KX)XC9zB+F258Vr}0!T-)T><|BPmFO3e$;?KjISZ9(!K$OeC*$#_#up3I!YbU1?$P;VTq8Cd#ZQ!|;2>o=2gj zot(-~9yn1K01xcWh-`lkDI*fY;HOeSlJk^g%12y$o~9S3mddOG0s@J+jn-hrVqYI{ z-C;id7)thv5CLk^>TrJt)_LU?zslPRuq61-`Sa)fqobq$Z=Esw;B-lWLSWztD&(maL}mx%bjNkE6*869^oSKP4{%=;uWCiV^7zr zJ2TZQz`t~w!o!e#S=K1%-&l4&QpR*-YrZ@Hm4J==8uGR68=f?%lmWMk#KMg)>gwv~ zv(-lp%Pzb_-0%b947c@uxnKU#`yO2W3)O5!$r7ge?dUQZ`K}2Ed{@+Te|r^{1lF5s z*q;x(+*e?%exPv*b-xZ$Lv48Op!F~s!(%x|mD-u|_pf9?0CJ)(#<4J8O|5tDc$;4I z=DhRsJh~=lqnsIgd`y+Z%Eq~6c;W1{P`!Q8(!d~qTq$&7)R{_9tw3!-r~DUs^w;K zlp`P8FeLjy(b25v;<=Pm1fpld4Kh||8s)NK=+wGB8ztoRjxjuvoKJmj+`O4&zf|-R zGkuWw<>)p%+^313oXmKzcERC^sdD~%|3M>bLeabT?rCQ$b`+Xw9-khn@G;^_l9!zh zci#0bS|!BCzryt88PIxF$9^>`uA9(;zoJ|OvHOKe>FHK?X8zjrJ%Cn17~_tAD@OM0ek{}27Vs? zX=(}@G*ZylPoYXSpKeCZ`SNA1<8zKFJ$_tJuEUO5@B0Ul0?uj>l7gz=N?BOs_GTjk zpcMWCcz`W<%4@xZc%z@dAsrgt?(n5kUoJBQWU@W4j^C5U*;r@Y@k_*%X_b`dSz z5X_8hXQITdlkH^qh#kxX2h4@KWH19b3mW6J_H`H*85)7S+Y>c%1v_&WGX&VcydN0$ zQK5@R-O04_l!(a5-VyID&t&Xpa7L+g)#$pho+Ua)HmbQdkdYm@yoG29Okf z5nhcx+J6Z?2C8rnmC%b9_4;{pU@B}UyTEWflqa{-ejz+iK0}2btq*t${$#g4qhWL( z8SF_A_%ZyGaJ5rGz0Rm$9#9e!zqeeW{>JHKhN54E`Z&21FBP@%j`BVu%(icU+U_R!BCwL zEm5eI*Zl>?-HFjoDLZS^278-}lltwYw&v9}H4G0P$d8VW%0*m_s=R**j*nTjSQgWf zt0(iC>BgW{zab4Rl9i%l3zXb7Zt8fzuO^8w2ybxxO?IvS(&SU$YHAJ!UW(EgEVcwJ zsIRN@ry6wJ8vLP@o}!Q$butZ|7-ZcJFXZKeowiDbXCbLIF{rQ}vm?*+}ku- zB%b01=Z3ulesT|8e7HN09D9NY@yW3_K5sn>82l-SQGtPlMH~t)OqS6`gT=`ZWqj2Q zI#UwX*DVqf6Ab_bg(W1qlVuWB@(nvvg02$~bXGVYG1%cIv7pgtz~^Fs)F`LBW1;OD3F)cLhFHTvr_ zt;OTMybK-7^+AkW=2N0gk-P=J4G3sN|ALiGU)b=PQ;?Lr4In_oDE|uM>go!R-ce$0 ze6kgMmjTjIBYD6Ck~36Gf_tLAe2Jl#`Ch2)Jso8)E|VzEob7n9yP;ySb$ZxoFC8`& zv;rzLSXt-6*0Ax3X(wW>u~a@)K0PopaxvF%sMMB}-=R>sHcY^|yy4;^H#fI_Ypk1c zt*iaQaD_Tpzn-2RCZ5c=RO*J8P*qjciFMepBaU0i_;{_6!)`vkAR=FGL%=}4Gi5TD zU+d4+gT3LSj5apY$r~&xNuY^=fh9@Z1JDu@5(+zU*vlF0ykK0FLE+ryI-gz9mV~U* z($W!KAxy7h-$gs-*VhW|ydNrk&04-48~5fq5mCa!VTS-$1XWNm1EG3gUZwSQ0koZE z_SV|k+T8iz$-=iBR-Wd0FV9u2(bLn<4OckldHZ2}Zwau#&Kj5Ryuc8WlQRM2!Gc;( zz|;5{Vr4c#e*C+yR##VjH-HuB0a-=tF_rn5mR3Bt_N=1w*k@W!%^GigvRfezJCh+` z`Vh1OsphHN+yIzOS2B;_Hh-a7 zVkP(Dg}Y|IHwnl3ZvqKf1K;qOilc)}IQd`}RR*+vUv4rut5Mm8{D}=YoZ0% zZ=hmf$>@W5$yPWyUZ&vb&{JN0Z&Cpk&2Z;VmDGKbMA}p(;h5SG3U0IK559_llLk>)&bA5N z@Ji@2Zotxu6-NZ6UAlFFNF(;CTFTrGG)Sze{ks?$#Uqo->_do2^mp(6{QbQAUGEQV z*k!EicBayTmz6A+_b(`7=X6W1b^-Zi(sZQq)XS>ph|7qIno9v`iSVQW(hj$twE*{E zfW9*O0;<{kexm@0Q1BqJW2&>Fjh&0apTZ!DJvUW3*6Zp7ldzpRencT|E^ zs5|W>#Cuh%h zmMV^%PC~gpC_0GC5VF3?HSF(-rwSP``E^-*o=}WI2XZa|slHO%9IE7(ev}Yykw{Wh zg_jn%{Q=D4PL%9s-AQ-qa(Y zD8*KbDZs>P^GP^#WANVLcjpwsq{QNN-(bF+)5-OvY=e1$DGsQ&<)m? z29;Y|Tb0TkisHRENTwRY;?My!at6NDUc|gaqCmho9ky5Wn?{l$`C$UE0<%J61fM-a zB4UyGyAu~Su{}N=62R;@-MeVxii7;6@;#%C`R+`m8UfpX@k2J_KG(xwycBH*iF}J$ zX*lYGt%@T#2o5lo#nzzp*W>8tVm7|10&`1^;Iowm!l3}6T2on3F|@6%Ek%3(&NFx2 z*|r4negNeT2&VcZ!vv7Qh#`as_y-l@_Hdk1pdFl$xyqoTdaO?>#DYBuzj|fTmv6hK z^LT&dmmL4*Mb=Wu*f;+GsycdH>j3iBM?G$VxuE+XyHU#1Px$od?Bm1jaY9k7O{skj z!+J|~b+uk|1h;sT4fG(ssWKb*nDLs`EY zfsLN5Zzl}QgcXZ7^Ax;RqEb>)EEmbaKRvy?+Rxzg#X$VaZghf0Bd0JI=f~d$1YL1# zgYvN*9XEGT4c`PGE2{*YHRA46^i)*O!K}eZPEtLT9F~!VJ$1Qe$GCKn{aI0;KAV zzF5WJgfBbPi?Ei(7YsWo(@I2Hh&VerIoWOYzC$8rae{zE0Z=bsu8RXjG8jTiN)`y{ zm6cCUd_-~sdqU9xK|u)!`Gug{m!De)*idf1Q5@?Bv}N3o>=khMXF zwWj~!qd7L>t(Zc9gc41^cX1rYn|i5sj?A_#yg9hi=GW`PBAylb9G zNzFja4vMo)$7bgOx3U2*MiQL%S6{*tA*m3-{QUJxJ5Q-IR_Nl1WsO8)Ez2ehGGcH% zgPnhqn0Nqz20#eo;qw12u;h4wcaxKoA*k~JB2a-U{1m(s%q9PS({ehVR^_*69!N6b zbYO>;?#@s}o(LM$|DMQu{KP`%qrG8RBPKpIvs?)|=ONp~lQ6&X7&{VQ2Sh3DoL)#D z>CtoDnbBa;`v(UnWCyD!!2&hJBOcvBj=ADc2?Js+8Ittq3rsGDMvU z4K>NkrLrxxlb|ihacl}G(7WL?eLu9lsOXuPka%j?mwS>Oqqsn2gl!LK`ub49ir0w9_sc zOg&X0GnmS9p$ExFAOqz%aR@Fl%H%y5l~#{%|Y}X$o|1%Zs%ws89fShZ0k1! z_E$cFzqYlvQ=&|?mW zA_UDFG7(P)2&r(+yHM_@LW9s543;IQpsIZ`+Q2L{G?x!-ilNcC5X0evgTXX^j2TB5qQ|(wQVY3`cTw3HT5ZC1u7}shna_m9@hw#Um(U9$nf|v!j{`%fJQcv{Szx#YXk9FS_tj6a8i6gliM!O++7Hk_64@bkzQv4DT7OqeF2M2$L@9TpK z>Wka(5Q2p%IWDYjKBJ_0A`m+t8yX~RbVx$c+>#*>N=d>hVOzjUv9U159W5r-IZ$Mt_r8}M^anJC^r2DO(~sx$ zRz|9j)sb@PYZ<*N>;(QmQ5^sk{27qrFjRTQ!)hPTU6y|TJ`HXvh>BzN43{0mtRn99K@CIU^?i-`q;OgaD%L`rfUF0jUvTM@~g00*lK8SU-D1 ziL7fo3rZsfz%PVFMInsOQ=y?!gJ2}wp`~qXfMt%Y zk9@?R>odsY;=Y63r%t56gW{(HD}X5>!wr?$Q==c-txGyMl!6vkS8ei$+i^FS|8QFq zav%|dt~7rt&=AB;O-+Gk%0LI4s5q$apn}3%`cI==@VP;oRkL5@YKq{Nf$a~XLi9%8 zw{=*!6tpQus20#@$nC~h484WzjGaBrhu)xkrmKnRs|D5;7Z`m#8b(3?5 zAZFx$@8#U#-ZIeBQYkaDADWw=Elj8C3Qfn?x5xcX#BG-ptkT1}1;#v+*R9qLDiaNQ zt~Vz!%K6Y10%1Tv6>*wcnG)#m^3u{6$Z}zTnptef!d5s?NSzZU83#r)2RH#*j+~KE z2C!(0vES#x4y2(ZqCZqxq?rstXs>BQAS4OEZp1*I3!+lBU(B;xDwZC-#rBE@qM9HfaN^Mt7~BOsJV_|{$`u%?2DIBnA*&7Nv;6_RAhc-6tp38p z2(Mm!a}r|)5ErYQo}8M}hm%JRHXkOF@qk$=_wO_O$MGJNcCUcEXO4YRFapJIw>|nW z{+9E;6lQ(p7yf+Fbky3PG@X<`+4z?(fle34soy~d1%hv$1a=|7<@Nqe)a}*^9766r zP?SR>8XD8nqZkknv;w!%!r?R=!*8$^K62N>J{QrZ`V;ZwvFEo;z@ETxJumuq7(w zd=QyxK$5bCqP83k1uVK7W@<6+^RSrvo3)6ra2qJ=c_2rj+XCrj0>6BDW0oS~v^z@+ zLaPGsH939#IifrtU*9h1!{c(;HsN~TljV6G>p6UgP~swi?Cbh$JIFxX1smKJGm>SX z03wNF|8A-Da`|2cB4nd6HNF(707_xO)iOnUJwVx%B_3}Cjz7>XoXD=FY=;UNwd(#r zqG;G#{ix-NBQ@CQ?5XC+RK#ObGT{UbwpT{MKjL(;yTOHreRyw$b48lGLD(=0J{&z# zNe2!NcCX|^_OJ_rSVVAJE}SXS1O%GWVak8%-=S)xI$q-wKn0OR3`!#Do0p=}_-f19 z0aqY{MN07uv%|_|`~Mok?`ybH(S!0I?eOn2JvaZ(1OYV_6&)>YZTi8vbLaG7m85KM zKPm5FldxlijLz1oyAD!_C^3r)-Kr9JGh!_U6My_KT)I^HUL6?k%Qwvp(z-s zSpr8$_e^UXbi*EqV37LIW%5T^eKnisv?=VZkz+;#VUQMNXCDpefOJ(1`w9#bawZTD z<S9mAS_ryI&c8EwxQx+E9L*Ar{6hBk72XJpWNCGZW z=MRI5N6Kv$KQ=yY34aet%>-r!mB*Lx*Imf2g-a=ki4&oQtH8s7m_N2skFqI)@_Gsc zVi*7fbfBM65QP{XztAI|8g|@LeY`sp7qnEt35qoQ=0}Cz4^oxZt1ahAQ?F2i!2Uq{ zy+jwt1dc}sTXvH{3Y<>++YA&2Bb6Umyf{FUX#>3_2Ha60X&A|P0P;F+9c|ZM&kSP= zz}g+WARIzYsoMWa0gFo|%UytoQ(dM*cJs`Luuq+D43dK|C86qi@Uvq*y`sN%#VW(s z(Gto&QoxBt$%YAFA>yEy2T=7cZZg;(wm?+g7%*?)HUz33H3Vz%A?I#m)^q?cw_C`r zwb)#Eow%6uu0P&tWtGv=DAsXmxu7VKY;qFj(6Eh9sXzTs7EYQSx+l&dQRMFmiR zAy;54AQGk01Iq;oH53Z)%eetaw z4V9HZL6E#3czIxOusriPEfp1Vloowc8OpkdZ^98k`46EXjDyjcrj`p$HH+;k_S>T# zAaOEP|NgBDh0dU*Wr$Xg6RLmq$eo^=N`Wl|+hP(p+>XnrIJ8Sd>g?^VI2Tha5XAt( z^;;19D;b-j1%_v1ZGo+?{-ZzVccd!6ATx7(^_JEhO7P(LW*|(ZR?byifX z6uHzJ7qLHH9?bs!{&rAcY1r7}MxarF!O4F&IWbB)n%^}hq z?br?;>V7Tg&1t|F!zrNVS(s%uS#Nf9Pm#v!Mc2W(+Q4EW@_`d4 zN%u0uX5q=|f*{3X`%f!x8tgs_WXo_0IQT#fq)6N6hkZ6-#Ul#94B*9Km;{^b@9f0Q z@0!9BTwPDYiPAxj#E(`73@tGYANSWV=skdPWgLKqaMObwm{BBF^akcC|6qtHM=AZn z#f#=?@3gf|7ZCfZ^gyyPn@AwJiIyBXVT1@%19XL=jzUHUh!G4O-5}Ugu{Fdq!HQ^; z8?(18-KN)x)g7cp`8|onp!&-gl+3Qu^Z@!q3IhWPN!+WzKH=x>nDjLSfYoU4-TSq= z2VS6G>2m73_UhIa>#E@vTq(J*A-7#_R5<_VaXd0Jv(;@4noSQ=5ojy?%`XMID1c^WPbPFUzvECddobcKnZwxBfv zgnx(L2Sl%3@|=?Kyk#%(y!Ce;h-|8$jKR0sq57Lq=(rk-hCnh%lr9K1Ji7i);>C+| zEnHatyCsD!EUA1RBTn!lW@?lyHIeZ*ZeJcOQNRGQ(H3a^6B?5Iva%j_U+2R4^R2K< zkX&r5h5V4*Ya_PcH-`vhO+i6HlZq{kjk3UsQVsVJ33TVu>q z1&2~m>&Xx;0mUJxuUQ5OEr78b+g{!}xxDt9X;ibIFLWUadxlkeqC__^%Lq>b%)9*o zP;`7rI9?#>W6?6`Z=D6k!`J&S4y)@83>pFCiyd0Oc*`C5ebfQiht4H;RC%%D{RlL|Qix zEk?TpPBUaj>>#`^11d?{Lvz`=Yfat#ZXO<~z^KKiU$>g4!!IoT0OIiSuGzMlRNeob zWXua?09FWQB93LT!&+%G)paf|r~-AZb$6?fy>nX7JEdY&F;HESl$1o;Zv@#~LjIZ+ zlBLc=iv%YAbQD6spsJ6!A*q9K!d%dL4i##luMdq`ELAu)%RX^m0OMQ9PPo zb?j>`)W+l>C zi^z{!?&u*&S#l&RkUgZN+Q^Bd!R>Z>Ip^!8dvqYY7`>YKia?dDzoxnjA%8(4qzVcH z`9fYd#OJG>ry3>qTPTem31HzV#5xEX^g8&Ut;KFGIdS{JV+JZZx>sW1oTh2^g01n} zmr^}d_@J+&2qJ<7P#;jfx>Z{pgjY3}KNp%TDFKv#fI@ilreZ8gni3LL@p{+ZhcH|bPtMH2HKiCX|b@=rlSLSKc_k5w1PiR*m8(LWphC`3(~$giGjnb$b+7= zo_)gA1KrJPwv$8$2g}Y1fZ?DXt}o=Y%mM3u$V*x(3kB`=U1)g_6~0SNL&M`ZU*>#t zNVH2+Mm1=^T!x89F31?wO?e?ZC>^G`Jb$5Oc5kU9Q*cq$F$0nmbOO6v4Se5Xs*$AD zZn`;AtO1+#Ve4ZljgVV$8uoD>K+4hyX=^9+LHkfXRsxie%~UIE^u=X;0NG;V#_!+H zLFya~WU9XSpgKqOGvvT|x~+l$#nQsU!rE~=@%0Yna!mm_Vf570aCFRwWK2j#rabCY z<4rQ2Yoyak#m3gE7paMlEWI2lpcU{!?-(MVlx>fBAvUVtoskZ6mjJfej&!71@ol?6 zhRp~{ST-WN%IJJ$QYrj9+sg;;9#m}82gM1Wn{0yDbHs#Y_IvC->nZKk@q4Ct?pRU?M94n?_Fm~)qI#z1bX z*OR4DJjZbO=ffUh#Vc;hIoWi@?AShRff>T9>|L2BPo5;~GVjwk!F4tS*^|p1f@sxZ ztZK8jgbuJ@6-|i&wJKZJ-q>S_mZ}QLFc0b@p(uYlDb~&ASVmzPWywCZ3sT8 z{Ew7D;XBu6_3h$NnQ~jT$7s#(-$~GnK|}jYYz2zNx(hwo;{AyH+0oI#K(XaUto9em zsH3@y$kUMmZNYkl(I5|Gypp!Iwn&kg%W>DDV0~Uhg~C3EZ}m5U?-+_DjNh@?3931a z;I+ULWE2$@6ZZI^NCf4kAbkE8BLJiWz@S%vFmrL(~VNAMaWynbk^rR>k(9S%Qv+<^_aZsI2IR1F^n7%oz>x!QuY2fI^)O z6o#mcu5~~$6narR)5jY@_`?_2&x%h0{@vJ_ory$jq$sbce@-G-r->Bq4!^>b@6It6 z@V9_AW(#F7@qSoqWE86`5pcnC5P+JpmzI|^flCfT!Cj7r1yyOkZPq#gDNs1{pd)>u zkUNZ@IcujXQ&28}C z<4+fb0DGRnEeRtE5zOIwqJSQ>k7FF>1qPQU=6V!Dz@!*xX<+Y&Pk0Fhz>@T!WgmGs zI|m2jfJA}OAoO>bK(uj0N|R)u*h)SC_e*I6$rE7YE_U}lM4*4x{R&0O3m?+3h$~#G zATEM1p!U`tx-zhBlfWf{DC2Zb#U*i@PZcu77m9!s%fCBw4?Vf(RrZ~Ju}8rJxNkD3 zLEpU>>!1(y8zMnvmj3gs@w=t2J^oxs2;iw&uOCY8fuW&E1{p;)%$lGOu_B!T7;2G- zd~;DY{W;nRkwJ4CIgP$sZg7iCbO#G#1U1!zO98X-*q{1X@p zG<=$XMQM6^dbU9lB3#-9(*mtS8Z?^_$8%MqzdX3!HIqZo(g!J*Wee&BNSSC5Q^FVJ zw5wMJ`8Y5lA`L;=V~K3yhThlz^#c4)E*o~PSg^9s z^BGK4V69!t^Gx~kxWjp8lDIZaSb+m=x4DoEs-ZjsE2`3ZN3eAQNDXx9Tkqe>N&+HT zJDfggt1fgT&U{FXi*2U@t~%}8139QC5|a@2J67$r0Tnh84Aj@sLqkLL%br}4t${X? z6r>p3(f!fYwd04j2wYE01~OK>*=GRxP6ALRI_UcV650Vmib{UjVtzl;UlPvVA2;)B zG*Rs9cW8!@w?8LgU8Jc_;>Y*zKb_`?*VK;oIU(v!uK}OGSY#J&A#mEqQ3@e3fxjhw zg>nx19;9`3lMt||HP^jx;R3cf2Y9C>l;02|1ivp{cy}T}29@E$C6Kkv*gA0N4$z>k z_h%MOk%~b9^p^*|!jmVJL|gXIMF%dY2)9e94}AuqJtXpxY3$FiQ>a36a7zYizyS!~ zZJiN_L1SmGiqo@I4ED?IkL zfI7~<0QGwUJDtg{r<$h?rb8~~t~7A<`CGn%`*@u{L;_=9^2N-sIAw59D8{+11q2q` zCSI8oar#hI%>7U8@%_@w)2Vmwp=8;RJ(vJGizqx`+EYCbps5$Mha{wKL34)T9i)?p z@&T=~2@4Ag7dE6|fseOGJ|e0YAqmOs)F+}G;EPC0Zi)4>JX|yRpN7AnNLl^=r{N>I zHzW;P|C#^Q$(eCN3bMMoO_#% z!_+5E2SYU=#^b)Str>pFwft+_fc`GU_llf9U_Jjtno2Zp0V!Z!KrXbC|g&bC~z=#6s6}(l~d_l38ev{KXX8v-21XtjRoLpzTH&@6D^Q9nR z>Id(8Tp)QJ0ogce1TQfdx)N>)?9@rDw282uLQy(xYse}ni~}udGSy0U+DKI&1sFnJ z!15VU-S72T#gFF^2;$vgli|`c19t?y?apKlAnjWqdIb3`@aVDqoi+OCNPb6Bz|XH) zj#IvuhOd#O@sj>EyK{Es^lzeb|6T038i?aU{(J?4zvO%4k%Taw+bO5=Yh8nuNh)dN zos?(N$?jyBqYS=|_NDr*U-fs)%2f9Cc^N;WMBX7${5MbeVn&M>&qqqRA?~Edt?u=0 zuj*+!G~9+H&q&Y}e{OTBk!iY5>{BF-cW1)=Tm1PuT0KKfdxqqX?KlrFF0{2t2Zn@b zf&$6y>;2{P258DqL*=hTI{LTwko|@>9oj%)YUoyM-kyi5n?=Si3e2z3N(hQxpn#8E zRNp?Gl9IAA5g;MWntci3SmPkIa-9C~WdAX0z_gY^FEiO>$v8Je&F(yQccI9MY-%-7 zW_Sg1;C$A(4d}I}tbkYgw6?bsIA30{wdhv09))A6fl`!|k`m9{YTtP<)Z|dk*@Xqs zuO-r{oj(_?BxanReY*-^D$Nga!!_&)btxN1{&gIdyu(UL`S9CZ7XA-c4;@G5g_6d< zggUPeLcR}A!PDE9P~?}yo^ocA7zo@H#49qT(-Xua zX@gV$+t+s?x&;??_3SAyR1(_#d!Ixv-*Q~P7Uj74ns09*XFK|6=T_s_hxiiwJ5*q3 zqbeRRzJGvbK{8~A{=#eQqK79zF#b6W6&w|h2MoxUsn98-*97sx zXuiK~(*ot@5!4y$-(je}6&Bk3+;Rj|g=0;0$hY4z;@g=BAShoZyF2o{^8&$~xCaUc z-_(PkNy30_VnqH2{^eAm_xv|dl}{Dr{PLgaBzXiS;(gPOBqd_SbqNbH+0*|<5o%KO z=4G{KlWx)PrvtNs%0&dRH5dFEpE_6iTL07FJec zK+&q9o&FRG5{qBZap?*KcKQ)JJ5iAe3-%RVpzoqkpmO~N^KpY)jZ=@l1!57C81rGEaY@twR1L$DXb4c6hLP=52b;ahZ_f4yeI}<;3 z7O!(c(csEYJKO=>OV5mN=?l8O5fs52xzYD`GTEJ{-Odb8n)No}>dF$`v)-eP(<2)1 z&;OK)(mV5{{O(g0!b*u?aT-UK_g(MLITw;W*u6}5N9$@FI()zBInFb@Fmi<4VyBMG z7~a%1VAjA4x>qGoF?tQbKb$!xqk_5l`9F;c#>@AXenCZW75JZKYpf7l1VbId8_OPc z-@(w2Kmi0OD^4)eYgbTLM&Yv`2aR4D7%)GMO{l}Wd1Qit7}`A}ph2xbpR&s| z!DBK=C2$hkZJ%l0bUu~KXC!>-gItn_;TGO?8dRlM)*|N%k4em(viI&g)OtzjDEK$y zmCx+)vFvI6Bgg;tAMRC`5)SqYIE3W?&ds_s5Q)WpI**#2Bz?p($#X&QkA)DLK=6Au zBr;rCaqR6^sk=b;pJVeXUu#9Js>vlz8H-M zU!~;b-J=f{-fcsfb6LgX6;0UTq0@dt#r{PL9tsd|e4s!~Arj0~#h6*@+T7fnTc-gy zaFu=IGI=l~v3U^i<}<@bhg&lIWH-BI33zmbI!B+oK2wxA-%dv(KHZRd{g=4gGn1vB zOM5|81?N?~MXB&_W6qN3(`uZ@P}LF_`8P&lL_!vl`ep@nXSe_yWs^Dszv#_C1?{K6 z;kdvU+vpP+{dAN>4gTy6hPQZf~%pHBWbujaij*;7s0JvRAWnuiHxIR^pn?l1(fnQbO4wEQOKO?K>FMdK?te2K zCK(lr%&3J$M9jhUs6voQ@j>f?uCZ|~=%u43k5(WZ9EXO$8t9!G%LFn!#UpEWMy1%| zym%tVp{LKZK6_Xew0ttX{y|lH?Kf9Cv6CDW@)~-KTSzNCqZbr#-+934C4_gunD6{JuL}OOIP;~~?yZ<#VFE4U zFOL2zICb?(m37XoA8PDRvN^}9rvWtI#Cg;D z4c-XQ3yq0oWEI%mHr2`9A^WC)`ar;yG$8Nwodb;~#lhcTV4T~M6(oP*bl0NqN`Es? zkKf+ywbQaYPq#Ba-968ucG=XB`aN&s-z)xEUs`&yccd7}iBTF$v8xr6B065>rcl4|#I?3x;lXc^MZq z;Iy~6-E3V>;Oqz;O3kM6*fVQAM?>xDY1S{iTg~dkrfsWFBSuvyktp7k5zdY45S-`X zMi9et&V;3y{7U8G>M6$9Usm*IXXMGMgbo{@CAIsy6SSJ9dRp91M~m}KXf!iji4ewL zQ{vh^b(^Vb$AO?!12yvPwT`IX{gx8dkFN2-&-9#ee%B648k)t}7n~AITls=7#@m=x zI`N#Y@mb`Ci7X-=L;=pi$>e z^Xg3Pvx^=hyee*!?;K6YjX&L)PbYAlCLs{zja+Hr{5kdhqqbf=u@Cu5Wp?UyIab>d zJB@q4yKeOOK9VfDzt(v-I^#oO$A#z`_x!CCg1T@MRMbJxePaXHRQ#*KSIkNkl(KBy z&ToFX5@bJM5^NA7`{{P1MT|}f(Ps12Qjc`SG1Iv6Jd)rxsmly6|EyQx-H~aAt3|!g zq88e!s|CGz8pwa(V#ixh7#l2gKYgK}V|CuS$ZIikRk_TLt7M}~nZ#w=0B-KRUmhx3 zfLkyPmZ4gB3Ylu!$h&5H;JRZY|5i~;b;~G3iF$}qP4HHU-|%k6iU}T31P7JuMn2$5 zAG*)Y%>1!@^%^K(!hYlz1Gonwnp|S+IImoEMQg^}#&_Fnk#TIHi#+n;r}H=(L^1Kk|ySGAhvan7wY zZYzCX-g~`8>A!j*N-x^#9iMyDvvc2NfBOc#8ymW`Vck@9X(v0hGi|p+5;v?`?xBHG zo=;wdd`n{j+EPVVr-_oR{U5mu3+In|)x0z(KcCm~NhFz1>WfA~x^kAhEn8cPt-Sm< zynNEQ$3M@!TS|o6sLKjkdG9zng&(GtMQe`argeqvNZM`Al4PO?@{4oG4ahiJrfz!r zG~UtPJ`DdV*CLVfvG+z7Wupq=9T};Aj=TdFcpIE|v^=>@$Ib#Byyn^5^}T&oQcca5 z)p1q?5|OLyK}3XvZ$L{>EU{u{Q7g@&W9iE}xL0a+lN$JfAdMH~b^=3Zh)}W&a#!L$7eX_wba0}U zL_j%e|IGJ3x{+F*A|5~255FF$Zv4W3a#OSJ9?_$r(+aGWO|l*5MaS!NSygtwKTUBZpQ=wZ^w+#> zelWk@brYn6ER}!ml>GtN6YehbQT5OPLGjj1>K!d~g&FBIhJJvyJPj!16aqu#`|xjG{fLM9!@r^y z$189KUKPO6${}f+ezZHAUQj>`B~srCAe(MSyO~GAucyBb@9XeMvBP~Kav@=aq1NfY z5q!(OpY>LWN^HU@UQ8n6=KR%{rkPorWUkB^?PmWx{i_9v5v)|Nn^k z3aF~K@9RrJhQ{Ytrv14{pL-TV9RJcIW-{vqGGuD0WOp_c{6CKna z1H#ZS%0!;S{J<$aKfK6w*2i&uHdcaS;c1}1`ta_GO6m&^SF8GXXoCDPpe%|2^F9N_ zX-sj8$~mx;NPe2wf`$UarU2xcA>zXa&747=yABY1@%*z$urNP>`|&*Z(zt#Qu*GW9 z@x$xF8DQzwNLOwVw@32xzps7@0$I)mh%kP=4R|mBDN|8J1*>B&^vMB{XBYUuY3_+H z61YKw68~&JnJl30cbu{%_PD1i7)`eZ*!#?buU%9`5V)@Mh~iz6-7lUSVJRD}YT@qiXP$z=6YD}VK58ri8*#`+uqirgQkf=ycSpOnP!Qj*r zZk&varg=%i1|yDK1*d%KsGy*|dDaIP=#2bg0lkdHLEE%@gSNiQvwosY!a&udXRw0L;Cp!Rf1vA8j8<=YP&5;%DYwcYrFOzGqxV@K za=dWvs4XX%W*i!2s9(DnWHBRds+~blMTS_owHk&Ee~~)0cPa`9VPSlHtr8uQV`08hVMMy&p zrUpd38p$1zH)l($=BTEfgMV+Hg|DE_Q+y7&-EW-G%jYA8`@&*KuSTT`o&7ViQKY~m zhD`=&X!}oM2F+SP)$~KaHK-94{rEwae>Y_9LPSg*T5^9s^D_>rzk9ZYC3;J>yh^E9 z1|K5f=H>?KiL`Bb3LhmwyZ;Y8vgDsfxBPr&Y~+GZpO8dEL~b`I{}KhPOiU#64G};V z0hoT&t{$v^2LD@%fq?-v-3&_(bLb)`m;vaHwl7ZzD4D?f{}lV=nynO{#{e5j1Dr=X z`roNSF32+l&BFuPjd4=K>)VY2gA#O+Le{^iDFM@@wcD~hq&~V z=W`sS@+m~S;upxk3biMgI-zkWaR9LX_rkWY(n$o#%(fZ;yP7TDBUvar*DCDHmbjI` zc26b6eQ)-?6BqVrGmn65GOMmuS`blz2&);g3_mFGwgiSUl1?>m1$pO*WVdm$$Rh$~ z9|no+Fcz~HACr2PRFOl;D2Gb?qxcWZP3OyuPJg-d1wu`k>fQ$jgJu>tPzsStgIm!`p03`~7cyia8NO5t>)G6hKi}5ROi|LqpO8~%p&OunIvDePxk_(rvyK5(;eMUA0<S zN93NteBAuHbQ%tGWMi;mfwGjIly>1h%04VFmW-@|vC+mBS+TFyycAwKA-`mLPbZLH z9{3Q75I224iTKCaVUmk@T!u0bC9l|`2%kXwBg7Z%FO+{tjb+bm?9QjKgU!$dr3uqb zAjUzvbccrFe;HQ`=hnq%>QHAJ^v0Zr(&hAJeDa=)?FD}+C!YF{Tov!Efs;-r$5oKI z_)~(`V!Qwpfs-Tkzx05nnd^No3vgg`aT^?<&j+flj2kyVCuXfcJ{3I25THfUff46@ z3m~Iu63H};OCAPQQgl#b0`}FV8gM^xTL)^v0BGe5XoZWXx}d}}Ha?DP@^ok|mdzEv ztXX7e7TEW4EoKXst$DQ#-saWI`B`FqVZSID|5_Y;+g21O;jM86fwU|$5e>A1--zGn zuBuRSoy32U;4hL-jY=DkH~V7N?iu@%JISfqmtJ$IN0*d@OCqGQv{bIDCkWRLr#i`* ziHm3+*q`5^!gR(ARlMWIsHrDXo3f2?w8kKbQ;Z@ufo0Rx$u1C{LUesc^II4*v046J z*xAd1C~Z9c8!|B_H1F$I0}qJ{H-H8&I4?LhTKW;R{e61a-PwWK8{z$J(h8;2pwR2l}>Xil$-FT72z4Il7W&MupIb(G>L6b=d(0vihV6Up7gij z2X_h%%b7$%3dfZ)jui-TT8YM3pWjf6T1On#7VqUCOu0tXj<{l!2{9LWOwf_mHL!<1 z&HAW>)Xmi*z}WF3<;0b8Ng6kez}(+SrOxE<3To&gE*+MfbaF|WVft!Rg_zjLewlzD zbIs+Hq85@snJHK z(659L3r|d*_OBla<`bXVe~utLtaX>H4`}h}y3Q(!d-sQCWe(dsr^F7CL^+d3TKuqG z@Fonu${4bSZAfm^&H`|3wr)%tS*uAfea4ec_JL=-XrU4Y^q|UT8wlnO!jr2LtUVRZ zB$jw`eVB;GMl&~Y#!io3HF1hk!Da98>O@-j(~&G1Bu@2Y+^mVk*&MC9pmz2-@>*m) z@QVO4tAou%beVnwnfsV@tlKz(iEhmGIZlC5JKQJQEhPOfHSUpRM#9qbo5AD_=3M(8 zm~AA~6LTw@LJ+8TfUW_sB6B}Lvr*O)1^LbX{=Sn9I%xEYGo=vC+k!Im=)n~~Pxw0> z2iZp_sW%iK&^&a_MO0(V% z^4y)#F3BPE4->RS<|STccqtsx!&Lm>@V>+t-!6kVRYUuc?Ej6WNNWGM`^p8ww$UbKK_4d0B(wY0RH zg>v~V{sOK_FVumijFft?C&QbylE;0?ve|f#T=lAa8E{ppAo9`@nz^Lv@11T@V%yJ# zIZmR{t-~a7>R%mlpqm@AHH$GjjUhEgFR zovITt&|5!e4#FpK5I+64QVH3kpcj1#7(zl@U!e_HytWmo z-D$i*F0ZVTJMEp3UlJ`AC9I5}>B!#VkW4S>TO|8|S*leorE#{!$5SbK1%)L=#44h?FXo<{W<1W2TAV5F1m}19{wgYD_p0-$i5OFls3(U! zB%#&$aB#V}=OzgsV+XQnZ|2uel7A?gSyPTv&sx-<;*9+JXfDZ$%a?sp8dv=nFy`U$ zY$h`CK|aOnP_08@mK2W4FLEOOVI-;Sx`oP5H8W@r=0eDy1xNQ-O}Vs9?}~8CCI{7B zCByx#9p7+yNjhoA{C`V}=?BUm?T~I0a&mG|M9~F-PIF2VObr4Wfzs^L{r&y;HB#Us zh(+^2t5rqpz^gtfN@i?hNQb*?0&}}I8iITf*6S-&|I=Pw*DdF=cj_+ z9%<2X6?}!#fZFUU>xETsEZ0hWjo&aj`u$*Ry>Y8`BOeKA#`%-Nwm`PN%U>T%s~i-> zuy%AL?>fy_{Pu?Q-Glx2EPOThU-f9Mjl{IApNE*}54L=Lo3=!1RjP?z_TA`cC_seX zbK0-HZ<37YoOkGa9-EPI*Q(*yfOMupq86Wft?A)|y-20wr#uu?4ySFZ-h*V0zI2?s~jZn-`l&QpELX+Au%iCLoq zv!jR4nP>jI6UT=G?mWfszhu`ReX;1l*DuYiu-d<);TE*JI!>ldKHcdZc zqm`^zh+KZS(j_yzeCI%~-W5K;eN$8HVBX$hzS*wSXr-^EYD*|@ZQoj?N>;m7c5o7{ zBJ<&X)Zl+Ns85q$E14mNQ(IjmBjiFxy&gDCRCIHw&3x`UOfCdOvJ!HBCFI9Sd zO0@mZyvykt!>-LeEcs$$2tR{MY+84O`-%&#BA}Sjr;b(z%hiFC0c9~D!nFILL}SRi5{w%9vZU0OuW!7GwTsKrcNe*Y_rmCey%vK zP;R;HXOsfQ>W&xii zUTi%wN&1@Ic@cHO+ykC4Q>~CW@NWs+wRzhvovK2l>LWML-xEr;_iS~dKTC<97^J)j zrMVQia^g{R+r)&P%xe*jPNt-C4)OGjDnpK!Sv)Nut5j`Fg?!q8L#V>r_@Og(8`FG8!rWcN)E5zj*%0FzivJ_otD7<-4t%z@T?dCJyWiB!*Y3NdEEfi#*v zn{@hx7qS9=Q%SZGrd)z|@y0c?C-PS5Qlfg_KkclhpPIfvkY(UT#ftIAciB_8$S^+YeTjKycJ^EG0D=?zx7eGKMW)z z^-{C@N4R7xrpqAzCfr|b`3<+m>^}i*$wA{Y<=ykG!3=I|OkYuxft{6y<(pF=@35Zu zilT$R?JO22ePL`YNXOU<0ccIgJvadxJimG;JVby=QHDtImmw0@8^c<}T0I4Uk0fuJ zNz8>%RZxaeX?a+C2$FS$)2I+TDyXZ7H#mDXSqqKKLOpnT2hc{l&w1_?YzRmT^n@Fn zZ!>Aoxj!xQ%Js$a^sEHjNMXYC&9wiRiJPy_`W91n5QY2POH?RPGZ7_r`O@Sz;x|vn zwKV;{kYvI`uy`U&10e>)Vpo#oxfl_NK}PfV0_@-h#;F9{f{>%e2oA+M8lSyC!?&aJg^fR z8*Ag>&<#LGwV?g^=Ml`=O0aA_A7FJBAiJHa_D?T>gv~LFo{|m1f+11&Ma$kF{Ky)q8jHQu^z)ron8Y8ti&Rkjmxx&H5IQ2k4HXIMW z3$4qD2bMtNTU2Ba}bpdStB1EyS=<83}yQ#xlrglZkA33d_wyp<`_WO;^1`eJQWQAN>>=2!uQ|+Jbp6&xj!~1JsGfZw-Zxpe z+RhA7Q^nRjHEW4GJztV6E>PgOVwhz)%7`wL)~>!8`v4FLGBG>-wd^RyFh+*zt2IiYHNd?=@9n#P`T|}%=9>KR&1M+Qj)*U%!_7 zez`CdCDXU_F~K|vPjF9!db5&h~%Mu^dcyhg=F?d7ab%*mvXFg(Yn{BviA{HMU4+PMSEsUh&zZVkIc zkqb3Cb@l5Z^Z=6LK^lQc@d1(I`M3j}o4it|!mnYvE|$+I30{y%%55W|>oCx|{fehT zEnr!X$TdYZI+tM6lp*`BgAM(N54XYm=Oml;@;n>8?(9~ z2L)1gIKyblD4ce&Ny@GkQ5q(4-12ScHrAfQ>I5SXa5ec(^)==*`N7BqQ58h?@I-&g zSY+*3VTCIetlDiY%@Q;%au$T&i>4&nzfYjOik8Gr>1svpd06bZM4&Q4k<^a4%!-lw ziBz0#l1MY-b=J3$Bn5K|HIr!RoVi=!?5lmJZ z1Pgu^em2C^k>j&duz3ypZ1Fo;_KgoVi6j4y2@H-eRpP()Q8gWG#C0ljj2SCyM|5-w z-dtJDveP*dVscYT0%NDr2m_idyB^*}z2t!h&9S59sA?R=@bui^O$up(TSab8 zrGzYS#~Fh!nqF;qyu+L8{N1WFM%oCAc#s|3Th3*nRh}_ z(^1uya!~2NZWl;1i7y|~>(&)wR>D2Bq8P2!gca`y4&kxe|YXg*gfq@pjy-JPB)*m^UK z1Y3gVp{tWF!CMSJcyqs=|DbPgs$USDbD+oatZcy4yCWpX+NNBGLxNo zH>R-d8PDla|3T1$B(otyQ3x|GioKK~WM#v2%D!HyPgO`;WrVE5e|x!HxdP2h(dUon zlBzxNNO|U&4F5&%zUb`mya#bcyUg{cr3t(hotq#9t2qm^G-dD7xL?)S8Bgkpm#%1E zg_nlBXE_XSzEu7+vZ*)veAie< z$#b--&z3ITPOf&u_?nxW(V{%hZNEa&+}M;Ut9=q>9P#MC?LXExS$ogSRU>Pgu;E{w zW@~=%yVBwqXZZr|>gSH>wnm;rxn&yRg5J#*6`Jvp*|8KsY3!7GVV?tIh~6M_i-Xeh zbs`k+~lVXmvJ`6%|%%wC-YP^uW;WW;ye$I#%C6fj}p@x?`rn( zB`-zSGQ1tl=ecW4?5wZgP9(yTGFMDv?iTc})}OyS)7SRlGv~p1^3e7Z8)?e)*7hsQ zbypOr+|eYhSShg_m^~x3)BDK1*X#++S8F*Q9Yifn_9@@XJjKoy$%~lCpY3Cnh_?1z z{m+d%*+FFu23GR_sRu-X9ld!E!-10IpDg&6LAgO7!~9EDdAe3z4}VLh7?Fj9$;%2O zdODI{*4A%Jj^;jDD}*KO-FSTcmeV)oVU?L}gZ_-KzV=YLrpviLac?7bNH5O7&N@zK zL3bf&6LIRYM1F)6bNbN1JT{vrYGd=V@<|JAVu=6iz0}o^%TKPhRx9iG7G1BWl)8dY zvgaq_(>bRvit^5)!EBnvD=Vv*Uny@*hl}ND?v{q$6=d>KmRS^WHJ>%c);p?m$LVlC z-YaOge$CRGMwiovD7BW0_+#FWG;sFXJ61Zr)z}d6v|66&i*c3FZXW5dn_(bPXtul> z3TVx)C%3b5tOj;<*xgK?!4oxeQY?R!{nqsA?QDi_rByg3f1Sr$tcwR9U7gpKMTul& zcqT*RDc)Z`%n@89N-wuvy5Hw+ONVfLvPxyd!$A-k9y-$}b^W8O#PbJJDJ#Z&spo?O zm$dnM=Gtic+XuY|ZTIkQFdsLQ$E!M#IJ&+4!_cIxa4XtAkNU;^?!5~xYhQ~#S>^EDkVAie^jvD?QN2QG?pxETh{ehseT${z zw9Nxr4b>%O*;2M?uE9LMY4>Kaq!Q;TuCAeyo#E*cA2-L{_IH< zEH!;u@qCMjf2MQ3sx)A!RVTj*$un{4{>_4W?RF{O^!4Z1z1RC?nS#@wGo62CPBk>? z-B^lUVEy4Qn(dtD78&W;_&+23w-U~fZ=27v<{1?A66pyyhtuOrY~}~t*ukk~bc9x& z5owAT1H=3)!mbXu`;wCGK-v975|wef-yhxn9+w6#ncbX|d zy)47Aj6zOQ{Ow0xd9T{7(#p{DJU1c?1+- zUfZeQwf!If!1zSISKe)y`pRzlr8V%(j-~;`RjuyMVFHo%EL0Y(0YRAivxh2mSi`+e zlT6nNW#Qbl0wxI=WrEM89y2g{)_3b9QI6&nG(}k?iE|HiTWDY)zP(v%{XF!6l5iJ7 zSSkfayKe|cOGaOwIN{q<@MMtv?< z`<|(B_3tLkMpMDd+Rq#C$lMwu>EBkO!_gqm?1;4OzaD=uXd>1cEZUL!C!-o~Pj%9v z@yBOYu4%Oi6$)5Y2&~b>QvM@Y6mm6nc>x1iq`_&K_0}ngxk!vz@SD&>J6Hoqdy)|9 z18(de6jNf8vNy!R_VF0Xt#H%oWd$0)iq@%)-3r=-Tcw@akU2e~ZV*;U1?v0pU)o_R zh$m8n|I*6~K*exF!&Uv$eg-%j8am8ON{b7h&%Jq0l!W2%dg&z%u^;$5^Ve38|1E(0 z6YtX?^1V>D^ZLOeVg?IYEtW?j4vbNgLrj=NCiE!Y%771iHF_YvbfM=lI)fp&jQzap z{WKD>9Xf3;r=Z@~yND4A0NQ7^QiU*Jwr!u7y0EYg%1y8n9MT zqv;HOgx3XT`HWoUf}dpP4m++th5Dyb_}b%1LN=2cV0zz>CWH0eLTIWiHV^N5Vf0J=p2*0B6cxp*l z0#@4xNYZcabi!sfONgP_bpUNB4TU(JE8adDOrMAPzv(mdUM6JEr6^>fhYzwtXp}1x zwqSnJw5dVnWu^Cq@hh6bJ3A0eL`6$2d+qtd!axs1^7v_Fu+pzFf9de!U;znqlkfL#bZ z7OZn_%(a>ZQ(`%sd=VMUJbnnjY5W1Xko1zvN2|%7Rw@fhZM(r6f#la+FQOCh}F4H@Ce@?KKu zapcT4n^h4qb1A4O%*J(^?(iLkUr1MGN1%RQh+tAI4-p`B&W-BcI(C-wpG@#p>S|`g zCp&M_6@uGU6%-|QQq|eac18}mDcPNP88XM9`v!#_A%FS1b;gYxMY`L2RnPH%t65Ng z2Ovum47N!wba15IY`x)XEP+n1MfO3_p(flB_BVXvP8nXrG6e~;T8fZ zz|M5G4CnC0Oj#7@!L1M-#Wa*yO{t=3ATzALM7#` z+Nrr8n{c&gd|AZG*PRwjdQYNLR;caTp4o?zKu<{r$wZPP{nCxCmtz`uBr{lM@9n1Z z>Mzp`z|Z_!?gCYp@65cP`cFUdMfUr3^i?-?`QO6vPv~AN5M4K~&D)1EcnxKVQs#`j z4OKA;k9}yxTQE1}u|m!W_45s9j#N8jyja5y)(a5Lr|l|RXkWmXEyL%qc8B|;Pr9gq zHE&WGD&Yz@qEIHrX+1C{K96nFwT4CKNIxjWpUkSM#*C+Y`nbkCU6XgTczsVB^^A%O zeKvt#7)jJN4_mKi*1N+37l365l?M#3x>QBP{{Ln7(Qzs#Gr;|_whSN1cHI247aig8 z0nSL2pQJGoJIMz>^hb>tgXH2`reRdFFDAm7N!JAv(eD!aWZ7|HIy1^2Z8?a^PNOXt z?ah8DJRkVGaB$7Uroy&AVW{mCJvEnnzkb8->lad$+Z?$`fln<4E0vZg6cH2s6UX&2 zrr2ReJ8_Y;D?Tt_w+J<9TX!AR^K@ewkImM<^nFLg|1B?av+dF|dAzC`DpAJZ z6J2iMu6r$4NSo_K8NF(GnAh8vEh7!Z9$y&Bad_ZIT&;9pGOEwh>f6~WIU1g&&U((IxSx|%1FLZ$#6zq+Nm4af0iuF z{!GjumfC8WSWpJ$HhA}HYDadkwte-WcfBQp$AR?!e;sPP)l#`E@h?F_PZd@#?Mx_I zf>iv!6KuC0_fNr@hyHjtSUpsJjA12E_v(wS(qS!(!an2<~I7v=TXl2J^I)YBbg+}GZC~qp5{7m{+w@`X+QCEW%erEaJY$z2k(A)rDze))_pZ`4W z|NM|%B9q7ah^$WP2*HRLts4$);STvW+Aqp;XcJVU$v+?;j?_2i`6y9M9+#ZyJ?M-* zU_VyZi+%EL9Sa`O@f+ocAVxR~tMfEz=7xBo^e>jj+nf;6@Q@}=X<R@P9Emh7Y-vbC=RImv&q z05VI-FR;_1qGmr}a6DH)AKtOGCH}#$nh(!BPc_2MUHB$x7)EUWLAFrhm!0yPrmt}f zJXl$t2~=R%@d|(lD>80lPCmykj4{4A&lJiC5b%8S+-)ejWl(7M10!7y&xU9`TIMr4 zc}dW>!Xy(21X>~+P;?y&3(M)`YgIEay#`Dj;XegLRMC%(`2S}cPHcGjJc-E+uk-q3 zBc0Q3AP|_+tNZQ*%~m$e7Y3LIoXo9zjUz}8=J+-D7_&d#peGB4Zb{GepM0$nTXeyD z`w+3n0MK&Vl^BTw`1shxM};gOUBsmXk?1h?lDP`+@Y#Qu-`2n9Sfwlxg|&G}u;<<} zyn;Sh*FowKQ58~ldRo=`BvDSJw>mYDsc=Ro$WM@^}@4ts!*WNf9w5IGFdnL!)$8&uYcAgR&aA4=YAK zguCO{ToEG~vcnFE@iBU5=9Wh|CUk%B9Y<(H51eQ}2V3U*pE+BP5V(;ErsE@}9+wpx zK0UKY`k;G5n^|r-n(7&Z9cE$((pdySq!dOxf8|E<#Pmj*zG+@I&&s4fUbj2O>2;}N zOgjoI3m=b^r|_O{PXk%wW>2@sg`5)@xkmwop2q;bY*9%Gd=XQp1K&obb)JN)E4RMB zehrkm3SfP=*Ns18K?`65KySJHFN-PHfVhHVRTtxbmfeTJ*CC&b4I5OF_9=mK9QhLp z3IZNVguhq9_Kfon6v7VEdzBP>J2Qi~Hw9KfWOHj!d=Rdm;>2rJ8M+ z5tlA$+w`8p9e|3puDLO=J1HsC;21R~9M=E`o1CCdmm(aI!}z}}{v3sP+PaulnO)+? z@jL=7@VkRZm>tEz*!C8wOEzAD^ShMJg?3UeTEdszuQP6JI)%y`+Y_BheH! z$t=7Hi}4pc6Lq%qbfu=7VVCR}!swwEdsEC!o%16X*7 z5Owoxf}V-6-9;l|#P8KuL?E59a<7KmB*nDXvS=PUQ*Xtg(ND>n*j)N)Z0i(WtGm-= z5;}LZXksk2uuLAkm7Fkpx!;go@cN+UjtJ?($$tK-^F>3U|Lbot&Jg8=&s;OAvk~Dt z**M6qqYk!#?q=cHVp~m{hy7Gu;+;J92y}jgDxx!)v3qUIJOxgw>sgnr7D4s26J5ID z)~axZeh|tp-`ym-Db?4o@4u3p>Z;tU$nMSup5T*c9@UDISKAS{rSlg%Op`s=(?K}k zb}yS#N0?X{4$?OXsq*o&&6+U@wBYc%YCdK(SyS}K(!Po_1+AQn zm+#r0M*iN32?RX-scC!!u-a45+d)SoWa|cs(xCwMt!nxYLjIqpJyUZQe*b2gs}>tQ zvelE}c+cvwi>)Ag?sx}xAzhp6Jx7=K%)H*#Vz+b`GjRawc)>6*c)OnA?Z*f-_)vV9-xr=E;Yf$ zckiaM9i1KX+>lreQOC|I_EXZEmVK)$`^lKs&PtCq4Htk;ls8RBo*u5{&2=&s(3jvs zPMpnU@=D?p{nnr8?-TK6i^tisoQDc63~2S0b>%PLotWHwy*Bb@U~0b&ORaq2IvXQ@ zrKHq?8TyJ_M2zS_%-&p|ZO^Nocj9Vcqy>v8GmCWd4{S`fz11-BvEKB`ug^P-PqC{C z0H~rLel`9STVv_j6L-QLzZpj-f4H#nbiMj_I)44?=xso@d{W9m7cKBBeRS7A38UoC z}SCy<%Qg0s^=wJVy8Pg>Qm-*M^w3gQEf%@iV`BCtR*M~vs25Gi(9$YR*~-eW)JhnsC!Iv^s2}}H{iprR zcVTHLzTx(t)rPxW&|H6{H|R_^ZT+=Ac?!vHI{9-Z<~r4AMDkvghF3?>9q+B3cVq(E zxx@$I(JqA?W|v%MmnG)$4g^guLU{Qo^juR$WkCt+NG1O{3k3(8KFTDJqG&SgQN z<4B_2O2QtVUP8Tn|IrP(pVchxhh#zt^S8be?k}Iu(jVfNyVK*7hPL|Ma^pO6B;|=3 zd1u+Wt6o+j%?b+Ar!q}?d{)KmZn7PXjza+d&yT6)FI z{{Di=-}g%d-W88!uZOfWdQKT(X^Fk&bHP$PmqI$VGW^m{MsKK$WOD3r8uUX(?Zyok zw28x*{mDrLoCvqC5DQ-@y>_Zs&$#i37d#31l+*b`c**_si*NODoKZCIx>$3FtLpbP zS~;fYr9YTh{EgFnLlouJ{ml4+Neu9cyVC)Jr?NjDDhM<=Jsl0Cw6)2^{0#w%BQoG| zCIBDIt4T>sje;UOiB}$g5K3k&B~UC)!AWKSZ(m0&k{B9)t#}@wJgf=sh5dCwHY=lM zXN@S7L>gYqau=>Rr58)VeZj+mPE{n$6<#PFT&jsA^%2GebD|-X{caVjqR@yz zN~Hh(?C&=+OP@CvOg8Vm#sb76Ms2ZLe&1rF!f6zuA1T10v&xQq;lh&cI=yLH6QmRu zV3ByOk16$$;@s0!X4A&*KwDxLj{P1MwS?xyfOcP0bKv^J`oY;7XLaI%^pVqARHRkG zy(m8`EfSmb^sXA&Y?2kv6l`A+3Qu-;Y>McaSQV@;7BB6*_Y*hG>evED64fK|?Ysy~ zYEs5YC-w!wqk34v)_;(@ZV8 zKbri3H*|v9znmDjlhzyKc`hih0;pWLB;(-W;Eegc2FSVdheAM&0ZN<02Et%f;h~%u z<`Gx-nbIMV;t*x2RN7_WB?v@D@xvy+Ya!MH)_Ygk1o3mY6G=L+nqab8DYWE}whzHY$y zG!X@ccf@-ELCe2FPdfom)&mVv-~`au9e?5+B+@B3=>)RPIeql!m$_ofFi`0^1H!i@ zKlqG_B!j+P|E>Vn zOW#`#Aeb7KGGj&K1r%hG&;J1VeNP}>_{s14zo(Yw;m+VG+~eO2sbJ(d=b4;9Q5A?k z8)B?nxcktX##9VNuH6FEOV;^36rT$)Y^o0pfFJzQ3NsZ9-4eAN*ZKD{XtKV81KAc| z9)@hKA*nSqkv;rosqI+Rgs|}z@3(OYpVlBecnka)wWx90VW%)CI&*h(TLsV=+7X}) zUJFDqh!>vX1OAv-0*yN*rT?z(W{IKH{`4(4N0F_XmKH+eg2?$FsQwGkQMTp@4|r${ z=mK=Y$HW~V>DBp)(k~Rqa}L{(fzR{!XliPfT#6n6rh;oR0`%roj=3gpCtC30z*!_XI5;pfLhKPJi<8d+4o6I{B=iHXp_+CZ;C@3@ zIo=w!`4R&WicX+s@osON-uKAfq-*7IX~hf(7cwz32R$EzYMB6$<-C)csK`i$P^T(E5t|(F3(WoM9bcqZH67tz<7f(1BCQpGboa z%{T*`oKHW%{F31Ld41rB-fRt|a^tW7dsE)XfqEJqYfs_4l%2a>OD+dMeqtV30n(KK zNFyu*wKblr|1>%9@$qi~*(Z(DEU6!;)SUs6>l)NBp{&b`{J*~5u=L>OunV4&&^Kuh+Mg`ZO+DUcHG0naZN z{Q)gF65IcdpX5-=<>QQMNs-jg4Nsr3eh=fSAW+D zf3^c?5GM~0&x->_8k!cp{~d)xh+17!Gv$*UZrgQz^+#dVW0Zfs-V9WEpb3yu!2S4W zyYw3*N3}qI5y*%5>o-bC^N@G$EZO+iA!0e$g^8_njDB1F*AN(ERI*|yNLVRlRV7FA z{#i#Iav?WlzHG;yhEGhWfQKpyz1x5#b`#_Hen94N*S8Jt$)+U?_uCX-Mw+}}waQ%q z`S1y1fdy*`XM2Ml+TSs4Pf0yRhB7)i78H%3(6BI>(P#J)Y*UhKLn3^Tb}g(dlM@-w zHzyIhZGdD>n5NzNz z9STr9G#PMZP=>BO8+3+*6^~gGRD&V=6#n9C^k-!KRtk-Fr1!5_=a;W&b5g_;jW4>~ zKO^*XE^0I4AE>p-hIFusx>2|kT|KHfNCXj-E_vu`zmOTDA;zWaV;wZRb;AytaCD|{ zh&Y+=?n;k#XR79C|F#UPSdY1XrjPoV>nuDGqc%@BW+Bh92M)EcS<_ORTm3EZ`V%`H6P-3YUAld@{8S}`iUOA{qEt@Nm3(nofx+QaQ@nL# zrOOMU>hNrbo0-tgV7VG)pAip+ALbId#Fx#;w+q>!$D8awVCF4+ibPwoM#LtI5l4o> zi8AFF#d>RvMrIH@`Wa^HGgjUVY3-6GqMlBO-`n_!K}#DBf9qDn`(ey)xx3*rGZ9a& zkF;1P{&2W{vUEh(KCjbm0J__+2qEr^x6L{NtyIWf<0~H0=U+SAOR@ZRd~J-XDSiqM zd;MqiQ0W@s{8;(qF}yCYo#707KzSg5SeG5S3x zE2V~$^P;e9Kk|uJCqq&Rw(RR}I@PK<3!dA$vC8wAK3L(Qvs&0po*^W`yGIe+UujQL z9!viHCHn1fR0{9J2v>9^3h^~D`u8Pd%UKmqxuBx)cDFvblSJ=~3JkDJ65o}wJ)eEz zW2yL1Up|#NO| z33JjxCzfafCkD~yFTR{&D393{i+&Sr#>yqUg^P@I`n_9QWi2({eWIFsT@AFm#e%?= zh@;%8>VJ;+2gsm}yeEP{zAs@mF65qYl!ms9M`xvYDLuP2sck>smyU2rt*OBdw4omtgCRx?agy+W8j;3FxM72OyhUB_EIY;=QcEVV z7rJ&!UUm9Lh;s+Z4O4; z+{X~hWW*{A%d)$2>*!t)`XYmV0C0?YdE8{adFLRP6?1IM^@))a`V<a7t-immSn57eF<`mG2Brq%^}VVg7@e zkY+IEQHOFKMUo=67`cpcBhNq+El*K&0KiKEVAjO)8!tf2JcAbKws^03sKG@x7Zn%7 z0cAAxwk|m^nK%f{y4ZOcQ~uf^w3&;!m19$CgMoFPedk3{49`LKXv^z#xgXL}yNl*h z4^!$0AFep6a^|O>?TNxEeek*w9md#T;IK6|9S?QFLG>Nc>1cWhnXqS@tTt`2hhX&ZXCr_JM-)AAq-frKFntk{LEy@884AjuG*E|M=VlRKiBot{qnBo_!sJ*-=I z;L^`NvimWsj{Rd9Lk@>iXCTJ5)b8Q6O(K}4{ZjKnzJ>M7j%zf2*FLuF7J zPo|m?TK$~cqimyOBV;FKvL%|q*MI7vPkq56EsdvUVDJI36rDEjfb3@w2tC$-0AKEy z090Iz5J+$R6=mRq1|Q9`Y>G|8-*9_pY^%MFc9}w|W)B>#myibSz4|RCld8G*5VO-7+(tXxMCgKa~u+KqouXt|QF~$LF z-Fj+-{zeUkz{Xep z{$-lp2(J@4=Q!l#4J4XQs)9YD%5y!n$nc_T=0NIiJu-InN*%tw4Z}B(yI}n|%|kQg zXo1Mk3#IIB95hX%-Ov;FXbBn)hr^y;-LL)>ibN(bHLC^A%ndKbG90<+?)M5EdKflS zZ4|)#VTOlBMjrQCz{pAd{HZE^PL7E)EGS#74k7XqzM6c^P0S^CaOodWz)0e1kP~^f$hQHiIKwBR6qsLk+jt~8;Eq)z~ zm)Mlxqw-?C-U}*X8Aa!uV1C0+!vx)*nsanLK8pG~WRmG*pA={-85yqAA6D%dsACU5 z(J579s4ygKK}4XRc;#3#hyV5wXc@n`ppb-8@ zc1UQ%J{R^bQ%IZtK)<^HThezQKh93@9j06z2PNhAGYOOIn(7JE`VyRb{p^~PIy8It zA5N(ColO-oZV&e|f17W+R@m>^5yA3yEEAZKR=H~J^UAPrB0nT9&KJ<^tiLO#qKnnu z)!qv=WI_?gb+K%}ibfsGxl;zE`RQ?Ws# z4+8(uA__Ch)VF!>{k3{Sq0!lC8cn%l)*P;0J0s5}D#+G>itLd*dBUfZovcW2wI|T# zvt}>p?!yH+1_l}riWCey>SgEJ;Cl{fo*Af|=N8fkkc;#>1KN`L+zpR~h16$QK}i1Jd0!_C6|x-WzcHUgB|5}ImG|zdin4k5Y-yyWFOO!>+|Sz-^vy&PTM^uP zk{S40!oJX5dD^_Ue2Y!)xSh-(G4_~ZT&9(jFe4axVED$TEBi zkWgB>J0+ABP!N#r2I=k&&usku;{5N<)j8+A;3K&AyfbUftXZ>Wt>-Zn;bq9vcG|bH zzxHkYja9!E`#}~Zr?o{1j-q-G)gvX?RYNt%H(Z7D3hU#`Tju2{n|EBNG&Ix`b?=tW zgqDkxW2S|RJLnK%xnB&~MeW7uvTVO79!4ekf?OV-ouU4@%Cn1vci4T1mXp42$kq;P z9^Z85J={(3j#cglsfm2NMp6SEVSEZlT+5I%hfD&J`&-Gtj+>F971rszo->$X*PqPt zxpCx2CuUjmX*efSVYR>L^*>?hfZKJ15O;r@wtRDo<=gRZKRt1gyk7R9-ARuq(sLuf zwqGB7y&5ULIgYW7EVX^*M?u>hhIRJ*wM+^MkVz|sSj`NzuHp~VhNn0S6mG_}in~dT zIffpETdM7ONW-*2dluO$q9W{%Uq4$MRu)sPEqj0WPPx)r=IS>kInm(I&8w5jCa-21$j8GWopm$S+7{yM|#_9UrL>9m2gBRL)}c7iX&I!V!zhc{$L;?3`uHPc*9 zXfG9~LzpvghF;7kRph+9K(jTOC2C<6w!p6Mw&|!JrE3fD(huu}A{qd^zz z!)eEsQX9jguqd(e-Zakev=aPA*vI{i)Wew;*dLt-rZz=7H+n+rWemT3HXRA3osJSx zY4f>!|BKGTS2%7*l+B^q==26r;&9XlO&1rl{rw{o)+7vsAd_#4EC_);Ar#~u=C@=L z?c$+R_^q-GyM?^c_?Qa609%;gde|T+GcNFL0)Uo8H3sMGz?mX{zOFg43hFd}d7Dtq z=GND9FKW2Nu~R8Xl(Bh@7v+6tPu=Wgl^^&;qd=ZxZMsc*I6~hYJfm*GZiPi+hrz=7 zc;g33h+utusNLCm1zXh4BNjrFsGs!dMZ$6BL0Y=OY{eh7{1|-)6WTjv#CeOy#A@ak zbw{}5(PO@|>1*u~Zf}or8`I0jbC3{iY#O>Cx8w~BQJ5=#{T5c@>kBHd|c3sW9r_iCE>=96bt zf_lf~_nss&9myfqE?kHP9r}XY?HpZELd)8IaWZj~K~k{;aj@w_m9Zg5-!?PAO$~bT z>H-bD!i+POQ-M&%hgf#%Fq4~M7LwAq&~!Z&mTSq(3_qz|0%^(NSSx}`;=7J&G1*Mc zaE}n*Y2Hi)py_m(@Kvif6lwA!tM($CN^~@-!(Kw?{+L!(c{_c-du!F@i;T0 z@ten@_pB0mnu#-fJ`7_Bve-){R6Kryhi6<|fg)FTk!3jy%c!r9*2j^4h#uvqMzH^y z@slYx&o@$Zx>L_GU+M%!xZK`2J=_psNM4U#wByN{39Dc99B0h;Qi`O7f zre^sTSQ(%FMvj!IUL*TfhNr6(w_n!R2J5njtUQdrn@LF`-Ue`LRcSNQRki>$eBg7h z*4mx`0t>php-10%+O8Bhd3cm&xj~TUUBL_}@Om2iTo|4`}#Uo#M{_?fGw#mjJ!Pq@`lIUY+>v*F4(%38X{O#RA=uLGW2MQ`VH zgcP&Fy#bUee{%ui0>cD`s}`}Xle8@)`i~iB8S8V5opVCy9BR?}H@flEtO1+ihu)!~ zVT7H}oHYhX1jZzu2BJ%kCXluuNU`>2;8baGU^j0U@bcV6CDmSkC4u#YcrM3C)Cx z$TnciWLSkN!8}XotTmLBqzAawOheyy*VtGt*0JY2+fuI!S)AuMXjir*IKR+2kU2lcfrlXJ8mvD_X>Jea z_GQt%m{E2g3;WntkSFdNgLtlSw2^PYdx4fC*+#n0npIqtdn7_C&%kadTSL74Lnsaj zl}3E1%sFevRsiJ(mP3Eo`RlT(mXY`8{^f6Pd}*57WUoiw5_Pqd(+bQYZJ-eoDbeH; zCr%By_QIG7Quccx5`{4i*;+}_mW0}sV)uJ94kb}8K+ob}2wFc;tk)0T zkms;1c&wn_PWs>BGaJa7d6EG%+h)KBG6k5C;_XU{i;(~ThqUM3d!t+O#-^r17Xjc4 zNsKaKJdQM?9b9)EfrH5YvV1WS_N`I!@;8rc=t4!1`4_y5pwM5a^&Dm{ffA}r0h~|t zFcRFsA(aq98DCB~Ka?h6D`u^P-z(trxYU)^exi^Ot$87Gz$3LvS1V)eJ~m^)=I#C! z!etHwhfFudL)m}mgRXR}i7OubGjkcVtjC{Gp80j!d9aQ$N(Z}{~$H6Zbf-4TX zq#-zv*ag^~&+<2fM%_RB;9^g3Kh>9yh&5@$GJi)#WIm;cRO#m3eY-P{0YQ2~0pp=) zAaU zCi6>)QsMjcqC!J+6q>CRbI%kX)2e3cZ7k?~~0z zgTM)IibK52WUhX_)A8t`NP&$&++ThYBKw}*wjjnLQavYZ?;Y{WVW+X?r)|oTFh!CZ zZn!A)0gW9n7vWx;BR*HF1kVJUm@Ih-VJRPNqm6g6`XtF@qpc^+VDPGihSnf=yg=h! z_?%PD&4-7Sf@{IMI1a(pu)Gx^G-S#fZX!Yze+xIyj4y~da(hVx8!<6GoP}UB=!r4l zcJcEABI<0)GE2GC;&W&6_m!&RT00_>zu{TStV9&~jAcUW)nO~>zJ0*ok2BWhS-&ZK zj@i!zu$Q4>{M6$a!0>noYFkY}&JismOfe`tOb`inRom`8TDmxZQ6Gn5q^wLad#IQlH?`E~}tkyyw(T&V2Rx zID^El^FtP0kIxAWL^@w`Duca12P^Q#_CWfPhstsKpHGY&-;`l5e_zAG`DjV$JiMl<%|KLrmj^5H$kMwGI~w^U*)8Kxuivt%K)e&rHzgr> z=>_fqi&Nc`sHT?bPKIG>qG1gzR=8(sddadxZTkUvW9k$cbUsf{ zLrr(Kyj##_7kgU`_C8CXXXoVAg)9Jg#*F6p+L^osuB|nux7x1)8gwOh`i|V>exNV% zL2#=JTffX#OFV@XOF!Lu8T(Nkt&@l=g{52Qpw|;!Fli^7DV)Vf6V2C{0`@s%(regq zq@$%3E&8<$E0Y|*B^C9QoMgH8&nE|;v`NF1xN-+F?|x~2l@M@>`iMM2vLm=;bj>O3 zyY%)#_x6tt$8`@UE9slJ^I9QYC&(CQ*`DqcjGZllIWU~UTLG0*$dgB@GRY8SLCb@8 zMyu*?aH8x+)vS!m;)fk3VzdziA!7wwOu5|O9BW|~xKKAqVI3N9U2m8U_ha0rpSX6k zPNEh!!dKjF9cZdlvbUo^5|A4WkFs9$@#^#zyL|;V?l(D3euI~E-=2ew%}b3?L+|3% zye7Ji!*h>p8_xApcsU798z?lq?=`1pxo*PoHJCIJi?{1MQ_Y?njCg&)aWqgmIY&SN z(0sjJdI=I61T3z=oKn$Esg7X-lWMq(s_&}R-`xj5lMlE*<`7$$3FU5s(HQJL(2WTl zy7~%_s*6ridVDtN2zkp;Tv;S%_N(>nbFo7sf>s^PjzBi|59`uL9XrH!ISJ2vpNE|l z)egO(T2j6BO}aZ|NcIs>87Qw|XVg=0IIX3?f$y;YlC$@jus(K`{dp~^>!SpPtuToQ z1dOi)@AY(b!kA<3s1G(XwOiyqqm(08cimYE-{Ql>=P?wE$h_fGaz_hghR2A6Rkl)g z{z}G_fQ@W!CXkWQDnZfGNPF<2g*^^L`O(k%-cBhy4L=Kf4qt7GA!cMhvSbw-FFv9e zxG%&+dD>Mee0NAihF*t+>W$(cOsJ$N*?H*BnYGSDQ@dkpIc0$8TrlkZI}-`=fv0Jx z&3`+TGnFoFG$=+=Q3dAt`hJ481< zG?YB=KqefMAHcBu4Swy8@jIpP0oc_m6%07Y%YgNO4&bu_PADO2V)rq*Z}XrUVcjm8jb9aXYpBnatTn2(+*faH@ey>s&qPJp$@pp^tX$*Onqr5N;P^; z7J9qc^uZ_dOR$r#PDWr4Igx||>(keqkw1qh4;0YhF^N9OX~N_*lu4mB@D^9|5=nNc z)DA0KO5VHO#k&U+e?JXA7LvIp==(uvE9N%B1tMiZT)kV%t5qr$Po#2ex@15p$>9U^~FLTezX79g`M48Ti=3sS- zbJl=t%6jFa6|cj_sV*7vL@kzj`y)0>-RtOMD}o?C%I|rBAu>Ep=;*jf@~HFC+^1Li zMMjkhEn5N$CwXty9!*Vhl=vP>k|%@w5htP#7Kz3q!RycA+pWV{_+(NxgUzvAno@?V z`2DiKu6Lau?1{fJ;Lvq@Q`6=OyNas#b&i>rhg#`4@7bZBz`3m_#5|@+ig|3suu6m7 zCJeg=bL|dcZ1-GIu!m#eYm{}#D}sft5H}kmid>sfA(srLfnIB4{Ym^Y6}#WbPNoUP)?Iw6w00wfgv_K!p^LAPvnB2k4o zO*AdKV^W`2sK4?mg~xBzBpcHmkm^c+27GfKP;o!F+IqIQ37g0BgH3+akHG z@+^JJ?(=eRZln4+uJ|=3dGtKxBQlnA(WQ5@8i!X;0)$)1am;`g%GCaT3q(F1;Pign*w6%}nyo_G!&zUcc2cLoVyDZ=KF$vpbT zfjD~uawt5d-^R`{srA~Dm*B*romKK;8Hp+9mW?>Xtq59V2QPWnp>Z{m$J^S7iPYy< z#Tc77CTS0QeTz@tl*tdtb~2I4aO@TsOI{XyC5v)~i`seN_2`WYdvEJxz&Sd#8?Jo+ zD|B+f_-a~Am4WlU*)S+*GGN^X5Z7yYlQk||s=M<|_<%{Z6C4o=SjC{1AGd%?=lo;h z^sw6$k+pR6lV&^d`B{#snM+ZQG*X$1U^y_|x%)@>o<=IT!ORE>DmV=pxs0zP>m+x$90Hi%qd#M31ky>)u-n=p>zX=JPm ztseWWM09@*2jgxKPtr9(1ZAit1Ecs=Y@WFae6Hk9pjQH}qPy_5(paZ{p9l}4{^M9rq95#}&Qn`~x zhc53-A@<2K^^)0>ecH9sukk6921zzxRa99|G6T1!O;k$@ABxAqXwZlQjab*T9snaJ zs-b}kJ@i3$Jdf+GLi=pCIuM!I)I0*%&OD3H&1wPEiXbqNAV9$auK$+2Br~N|B|0Au zsz0juEwvJ-NXh?HdT8G=)Yr3~XDtpcy_M0i#%d~PJ;`U~%bzs+(3Qx@&&z2+vWVg@ zRfR)85bxKlluL-Ratd~Y!*yPl5GpdtunN5){G_DGHy<%E>Q_s8B%*5{L}fkrjPN=k z+)~`0>l>SwWxflz&_Ak=-OP82i?4~Q+TRbkmnK6%L}c?A2%31L$ZNC}C``~UQu!o0 zs`>43+e?+Y<3fi6@&QGuM7QZ6O=@RZSv1pKgYx_ zd>9i@2+SV&;IaoGoSkl;_ladoc!{Z5$)^h~l|AC}7PMNR60LB{+L6cKWx%q3oOVn( z^Xy4q7V#B_?%NHKNBqt|gM*{_hTIfhFkscmrwj?k^C#xzJahfkrJru!)oL0vghIn2 z1}p^N!$`|@U?C!77QafgfOApw7<6fGP|4h+V@G;xifY-I^_Kb`zPNc2vZLgDIm{}W z((Cor=WbKTGQ==0O%N&j!u$zJ!TKB)4V`NPWE;gD9RqV7C3pB8lspIu*9&MGh8GuO z0O1%SOY0>$lmq(~xF_F1fdDHfXE1atgksjes@6V)A|NkjLvsui-h*sK{JOe2py%>w zqRBjow+z2;-x$7ZL7TVCR!Zf(1It&&7PiHlQV+@ zaU`+uj!!0}gu2p|5^vl$Hm*4aP;gY2tnFXgPuwXF z`BwXi~^9o(6yI&U!a<*%d(>g=q6YUG6j2WV_YBB}m6pIc8A?FJ zJ`nVfnnu>&wfY_8nh7ziq*N6JzRQ91n(b>(esd{8Ju9WYMEsmX89M#x-E*=O9>Q0A z+hkUU>RP-@LUv(xu}n$5%G-WnVFlR=un<|ss!3|9bj>5eeB9kPys7-w6SLT_Lp(yY670d(`(vWm%3lR?i?mjCGVR0+Ky zX`m2xX~b_|!_(G^@gK^Q6)2N=0p~CtwU@AV*N~F zam_o;nw{O7Q5het&&4e#I9b2*fG`6_r?$YG+Jx z>_Bnb)+rk|I#(z3hidbtAmL3}Q7N%<65#e7OXN3Oq#cQHLWx19W}2YJqJ_B3?ov3j zKz$Ei*0SoXBePe(Jh8T|B>Efo9PsVKeCKVXKS3}gPLvNnh)M@Qo+(@68t_mb?~4xC zY2b}urEU^MRvx^Qu~a`gD?aP++^1d zsXvq(Q+@{V`yva|mnR2jj)9HCG$eg1H$@{zQYpTX9%&t(^s(oAnEt6%%yC$=`pXzw z7mV5Vm3Dc8iIeYJkw`(h9CYWNHU(56;cfO1X!hs9hb~u;S6J{p?VuZ^=EWY+el4Y$Ar!CC*GtFNnd%JjN_QkZ z8I#PYFlXUu^4BC{l}~R7*3#b0ZSTMddr0}3P&(y#KdwvD} zkJWot*`C?B5W6~2f>MPJb!)u!C=28HH`w*0ZpQv4hP-Ip;q};tY#*u>7-;p0*Vt}g z5-3-35sYVE;X(7n8KTFlYrj)Bj*`5Wwr>HcSe4>TE_!+rMzYhQ!|qdzwLNRUH`G_X zluvJHj*R~#a1Ad!e!MrZAKBgZhf2JRsak}Eo5C3O z0!(c0>lv&fPG?Lf<4xi$8<-^4IpO(L7E^CUQ6uVSzQ2^xUBmF@IpW)& zDSLXQ%0VJarY}2U=3JlGleIaMma-b&GDl@2hh>y8Lpo2s;$Dlass9FCHJY#twJfb*D zq}A2~U!`t)$#HP86&S{H$^fPhTTGmCP(M z;?~!sA!)6CNxnz@-0D}ynN>jo%H!8BZ~N>+C{{@Ek2WRx_^tAUIH+PC=El4Az=mW_ zOcR^AE3aF?CH~7@V=n%_Q`1=gHAbWAS+Z}OtV>7L0IfVC^}>qDQk9G)@(7heU`O33 zWn_X^Hq)$o1- zl-&DD?Ay`MPJ3!K08=z3W@wDjRnBB0@|bPN4!cXX<-_Rdd4Mk7wQQWj`8tMumJ9*% zv(@xC>)eP-&;w@i4$)lviB4bB*|GuNkYZJu)nTO2z;*S|GqM@Kz{3Q^~?~ zDkiy!#QWJ% z5bD*c#MlNo7pL;}sY`z=QKUHsX*cK8gHt9uA(Y96Pde%x(~d+c8>(A!5NwSca|Uh2 z8?7eAq)M~qe@91I(^?ttoaR`~f2aBVSkNdsU+KK7>oBgC6~G+2*5F|U98SCOy76Rj zV2Ca``OJM=J3BnLky^zi)_syXT+hXAm+jOD6WS~J>I;5Gp{o_OEOKW{K8i-CDM4cL zuv-c0`foP)w?&Av_`(6}eGql_-4~A>DioP=NJ`tqoM~3e4Gu}TO?K5^`|axKfD4KM zyGndtr5$!Hv08lB87EFd*k^=8s|TUwl-707cn=8S>4F4z53tusW?Y<}Lg}(R-_vN} zGwTOvmXfwDaq71E@Bz?QL(}Azjzd?E=NiOP>u6cO;`>@0GEY1F%`q;;Fu|TcFlm{c zu>`j+`xDVV2ESEK6h$%s{=FHrE`~MSRiTpA%gFX+RR@~dmO|NDEMmq}qp}4&9MPIG z5{(3Y%Yv1(Cl@){fbe!Yq<$uZ00kcN+65zMnYBYa=p723a{QF(;pj6lt-H;gvkrF+ zn8fki!HodsQ8s0P;TwfV~L@@}#45h-hYEr}C-;>m;BzxO_$xk1}8s9Z} zBq~6(5dE#H9aelxL35~wEbAwToXQmpMO3_c5-INIv2gZ%poRE}$*hrSu<@VFxttT*Wl~PqpPhJi&G6ZFdp*Orry9j-BXuuFyp$;1gUVoHx-~-84Ld3lKgC}OF ziq_dkGQlAxx>QS?!BOq{>Y>mmvUJTV^YRX$rOJ6`sPUWe*YHYQ;W*NKt~-w2-wi=* z=}^J!^LXQK-ZOw6GtLw1%Jca30i|8f<9ehxYA(}q=5eHXtf&vjX2$%Rxb4^zZ1afp@#512|X|&&kd7C8WVR-%RxC0bRIZ4*A;>!o@M|->$zhLD*^}m)<+8lvp;yp zw?n5`CMAwX>f@z8J3RF;x1sl_e(c<+S%)*bT7{(l#_Df-C+RMXei_w#)Cu$IKZt{; z9huo_BclOM<1PT~Z#-~vaJ zhi8@p94RTvpZWB}WHllyyzq6{8xybTicE>UbV?w)?w(n){UW9b)4K8Wo9dmLiy=G) zYWBE(C}PI01g$5rim>jN_}pDbo>+0TOgnKsv5tS`0(^foycC4=YM_2Plqgjj#)16FWNGbY{>w_5fv4c=QKBFe;x)u zS*56f5@!7Yw^A9YbgW8U1&V6BSkd%KgassQBgd2J)C<)`j~`Q&a7l0y^+v%(BMonu z^|rA25FO^F6W?iW(@e6Q*AJ zs3H6ftU}leJVFgA@ei7OL+&L{XLQC}68?H8ChOaW`aOV3VrXiL*L3y<&`3h5+4`r> z-_iPakm6bP?>&~Gu7x*X-4HCph6E2c$UI3c!F@~7Wkoh%)s;t_@qW)#&5pg(`G|F> zxnO!pWL|}5+pF2Z_0rg%zHOy8I$Kp-&sd_XXiS%2qCY43NgYMC><@l|2v=5lixyFx8XdLQGR)64Z_ysMnY^6uVdqRskwU_Tyo)vpJ`U=19=G1mrBc2_&K+*4j z9`y$=#qaH8>M&t!mTwtSbcE-bs}%IUUV^g_91WMDkiYGp~$Z%pvD}`{PDYPn?uvxhyv4 zY{d7}qX=1}fJps?ehA{J`PbLOVmRexZ&xBwx@FMQ5>6`N=^XgtEej*T?O4j7B`x6w zXSQB2?BapoIW!RY$}*OLP=0iNI?JLa(Q*CMk$c*zkbr05SS86HbUcTUN32DL}c zSL>;~Zo3445P|yk9^?(@)Ys#I7zqFc9p;pfmT;Ejj9i!`bs(_`x$)t(Rw@1t>EjmI zb|-i~SSTvn0dvU`g2B5Ywz44UAr}5JWgKjKlUeeK*#-rljKn{#4V$fVf)Kpjb7Y@N zZRHvslpQ^gCvWoG%pzoY+BP`95T)FntNmv%w8jfe6^av(?fz@cX?d(zjQ~8J667C0 zt^qQp%WbW;2q*)6V&ZQ-Km*=f`iMM*X!+!+hZk5JDCE32p;+*Fu&5DuEFpIJFrm07 z99CIrwlBq-guR~^qfE3|48RD!Oy+#vD|&e+sqNmVTw92$u(Re9=4?%Hob8&5S{{pA zj#xf~w<_@L?`ki0WuR0Txw<>>k9qqR<}ZI#>v4il7^tj@H({5-UN7BD%r&$PEA4(a z@aSgfYZ3hU)~DHM$;;u!LfqbUMGpo}kih-dF)SD|aGwup+HVAT@?efA5K=2_PN-*v zSlP?9=6-!?Y3y4}w&xVjhq9c1Hw^1D30S5TGCDzpF|JKZZUNT<`Cfd3H9Oy&uZ%|&)Urh3!SMY2BCJ45Y($W)B1kk1Y zYEWJZD*M*e&$*umC$D?_SI^#Aym%<@`1e=<tOmK>w+1Mxg zxItSbjZf20jhM|XynPru*=x#wfiO{YL_`EzA2ea@T>(HobdeKqUPrmV>=uSie&LU! z=>G3-G{hVGFmfcAJ9#epM7=JVc#(dDfjQjY0Lu41J*Xsyurm*7{&;3)=9m-~%xP0- zUdF@*BL@fcg2|=1XAP-2h!MMg`-{ECc9yZz zltZg;RY2&U0j3E2578g;T%r9vmj0EBCHMbO-G5{b)$aeHV*fABssD#wEs}5m0n)Wx zhu*(JuT;Uj?1%LSw{$U}9E##$Jwt`tkNhC)c@utq{==37UzSDVAx0;VTOoM-li~^u zT9^SI+5=uee2`edR8Uw*v-o_75n7Z@x%m8_rT_0n?Aq?mLCIFlucX$~eDIII)0&gP z!Gjq@vGjfc4%RRS-pItn#M(Y#Krt*@IXUQ{&Y51J-w+55e_JI1^5Dh5(XF)Qzb^)5 zA%(NS7~~ID2GpL%{JBJLzWn_j8YdrNZUZGhRqh9d+Kh>W(UAomdiE{wg`-H7lc{bE zfW8fiwC@jUv@@1}{?nQT%Y%inbF^dYJ|&O81WK0q@{clRL{jptyGorO55s z+}jHo-I93y`t{5j2-nkOc>G2Z1o7I-xeXzn&S9wFz|(K{ccSJD(?Z*72Jrn~z0Tgu zuv^p+f*X1twVD`O)d0EA%0jPIhcTh znN%i2B6qmf+-+|r1jK9bphP3DdHH2Gz%pYj=?f$RirI% z{xiUJyuZbyku|RefB(QY`VPvWKDu4~x$*yl4(-xPjIdDWN0raednPE!11QrSW%dWR z^$)>lC|U@Ll;nffPXE-n{~1;pcfap8k}XRLbQ=o5&gyljGA~jOgXiMAe!+^PXL2U( zR3)dnJ0k7QWf=gclwjG!IaHJKYUulXkc6(y$kXZX^G;ZU5peFV*m`pkT2$4qLN$KM z!}qHPH6(0q+(^atRDQY*Fgp&~SV`j>n7e7XXg!DtUp?t;|>X;!o2n0pi+1c5$ISw#4gxJuZcRn&$05FE&Am{6Vh%KjR>K z7zY*@=S;O}STqSAmfJnphAMVjz3pRek_S4`ZS!kk zZaE#H)k1`h=iAlW2bq2XRl?FhAkfADa>QtrE#O&udI$1<#uKy*3D>qI3juwFQA0z6 z7qDU!^$kFPb%2w^!1ruir3?kyRKN-PL!X=unopvSPfoxVT~T#HBcGfFbPV`{<&3U+ zwQ}_5HxB`$wg%AiOsj7bpuEkkvW<<4gEltrGOF+|U7uh-G{y}e)P_J7W3BySYt>0* z<2L<+wgEqDj{fvddV2a1aO3U@uqUcc>>IbJ%Z=I$dwP0QD*ova@k1kSKYsjx4h(Ro z8`z;jz(@ph*S~nH5MN4cSlM>p2vrvp=56;@j-^F?XR=aDQZkgeY4`I#l7{d< z%$y>A$KSzjyHnY>w?63rEx{%g(6?BwbVa4`Ia`3Xbh~57v82LUuU8U_=8pAwfa%2*0>YFz-9QQO2z&h6pMlL z=M5;@>#T18i9qpS76Kz9^@aCuLzoW@sQ>{$WpjS8Nal7ASb9)joDPX!56Af7fOS2v zKi_l)wk*IBy?`trEF@$IrqFdex*x_`??Wph{*M*FEGjzu#W6@LEG%p?wRGlHXEjO< z2ep1Ik<_qt7c6N3bkR&G6@OMD0%1{Zv8UtB|E?`wl$JFVz zc-3*+Og)R~#$Ma*04deOK*t*T`Xp}mc`|X(VMIovfHNu6KWj3e{Tb9gb3;r+y&XZwv(J#EuAEa{9&(k?zRBQPW zNF_ccg}>}?ZvvJ@Eny*$uE@d+FKBo8omQo%uAuvJo#Tq+yi*^Oc;h_OVhjP%oPj=t zru(lYXoYq$g-?*OJappK=7W`CTLT-7rk`Icm{4b1g_)tY&VWPH3QV|0Plj(gKJG`qL&Sz38<5+lc^=>KWtK`Lxphs&K%yFIETOj_L!Dtc-`H~7*AM9T#* z9tU?ZU4bri)L(&FlLvI;DVQNzPW;e7q~U{hQARTMywM7M0D@*gC*wp!Gp%QwOp3~6}e006Nzp_|;5U68ebu}AKhR^-g zhVks}{Ni0Y!@9ZISwM?p(Q!aPJH?uivsVhP#%~ z)|Of!C67uj@iYY;d=Ldu3_EQSY)g6Q!nevJn^*Y`UuPrFuMtKpYA1O~!wjme!L7@WKNh zJC2!csFHJ*VLLO$e+!<2T+q$7`<*t*s8%E7EBH^`xwf;l%X5+A+!O|64|*UF_-}n0 z@Q%Pg{2BTq65+qp{{NnK{}KK>gv`N0;05+fd4LU=dXNo|N5o)hJIJ(ci`r4EF7DLy#8w9OKZO8$$H4W zcRM_OQERK=+1VM0Y^eGAwt$6QQ`I++%q9*^1+Dcs`fWV(cR{m`JnYHI7o|pbvepx2 zNCF4fJC&29?sM6=44Rf89mBU`0~Fh3cYC#f$|d93-GvrC{|k42_h{UQ7XD?AtR8$) z=&`4zpi;L^7C_GEHo9!-lwYhR8BG8Yc%)|8(<9xBXim> zG#id`aBw7~rV3Eb{_O2lH8eD=+=4o%b(X_9&!-U{#7aQ=x6@pJ%9D$m+b#7RAShV@ zG$!%J^cL@nZJ?7~t```OHzOD-2u5rlv={JE{9%{O11=P>uWeu|-@opM$JZa)JAwMb zP~%88Vf$dh0yY~Q{w~h4bOHBsWceISLjV#}HM_C+=l%lh0xMPa_zyjiKz!)DnqsC< zITRRdzTggoaVDUtK)bVs%^r@Gq8nsj!-UGNv(*;zZOV+STzR?6jLBiKq$Agt3#I7# zi=OKR(+TInLWI!Fc=75&ehQDUXmN}N1MezsF3pX0e#FxTy`Ba56dwN!yg8rJeZM^E z@wXqLZYv7n(?KvNy5f!QwzYI?EEG1KZEVV*m^QvAsf&@isB(#nTX^wvz}cZ^WQ?3S zhe;c>(YR+<2&tP}G2&v9Zi>F=n07-sirZ+k zrP#Mj^@qTF^;4TqB{8$Hl-91ry62Q32Cj$GW9noBm(&R)Iu7t}N)EIu=6H#q`fonHCPVZAIuYe{~KQGnE%qW*Q_B2KN z^JTeT@>Gc*tx9+deEh9`lv}w&t-`{G68LaKb5a!9!jgoGI2WdOEE>oSVAEMa_|ps~ zhG|!}c$m7%@YT;)By4J0E-tQZ*Ez>-(iGsR5)lzm%FrlfjDg-m(|jab@E{uuaswfB zz0=wyInhmMW^~5l)ox4&_R~-)X^e;`d`=CD-P|L6GmyYXuT8fVFXvj(q%lmAosC=h ztME6K%y@a7tVdbJqN{-BYO%+A^~(sRi*1tpLDI&?;U1yn^4jha+_X={@v|i99E>sS z7dR!+@B6jU|!Zz0Uhp z->VOS^xmJPUCU$FBH~In+-)o(A`EFY*SB%Zg)}4_;3405|E{)3;KkZyO8sVzLu1e^ z+dRJl#vpO=fGqnBB<%}x^}y%Rx%^dJBvtIrcj%SjkNkWR%`yEWiTV@hg{SW4t4LC? z3cyuwgwq8MgF9+FE>H#P!-C`Be;P(519iDUT!Gu9ZVsFVp@*U`RzjKf!6h&O?as_H zv6C;|52}ba(?!_Q-mB!cO4gCasMkcp$+=WXojnTJ#?jE$uRRQxbdwaxHfti($M(qJU-3aFdtT*vWt=Ifl z(TKRlPkfomMOu{R>&J-V4=hF6Cqye5Z}9?QuC{m7d4hKYt;H?UCcSjnQIEQuKwrKtrOs5NTNK* z4BIP|`UP1O7nT>A*kC#}qziU&)T+05zchqtv7Ow!kdk(Pr^a>Xe5SrwsxN*mYBc`q zbPDOl$cT}87;MBV-w9us__a%8 zjq=`XjjM4wZrk*`4UXtc(4hm!6!Qgq?_DGbj zu3@Q!)pKUDiF6COvbN`SWTP|7!{-Oaar&Fq>}==pJ%xAND%FnDdu%JE1S)Nt8byjp zM-SvRj}1IasFYzKuk;@_cT=%Jk>9-xl_c4+{Qa3=19}a7*jyv!@i1~15}wK8-$$;E zu6}72-UkyCi<+e?)Yv!L;UkGO3~4zhw~Xqy_w_6o?C#5TFFZcIQL3x# zt57qR&Dr;BtiOv!(QEac?wz0X?Q0Lxt8?)S9C1Tf^(<0HrO_KZE;iya=ac=krFB-L zmq64e@U`W&=N|QGCcTfGfOi2HJzHZbXedJ+V(}O*Sxcpb-oO$&5g^s zUa-ij`wITjK?&sxBX!3mWl-71s+_oU&iCn0-$89wDJZ8yIu=r$-XRD}Y`W51yTnr0 zp3S3fGrBLJzA~CBgQt;8xacF-xNzK1J**R)=NA_JRM|~u`#OQh2eri0F^N5?DQ~5Y z{>Hon%dYiiSG*jpn<3t{Mi1#CNv6+9-7`;y3GnL_F;A?UtF6u&csA;{SXhA^((8_n;JbXu6?CZ&s}-$LhPJl<)1j> zFRaApjU4Y6SL0hk)jcLEG{L4_T(6w(HZ-D%Eb5n+myW1Uq-4dTrY4*? z6sRVC<&V*`RvvGY*SNO#4iy$n5d2UPyMol~$*m>h%%51b+RUZ^eEtI@AJyPz&NWT-VoQzL)+u{@I$MqUlEL$WbTwSvFN`{#m3p#X zC9?T?syM=Z&-QdBa~x*B4ii z6TLOKJ5o_NvKqdn1DrGhE#N49BLMKF=9D3Z;o;#F2C+Rqe{O1y3g3+iN4dyA26)1_ ziQVP92cP$SwU^`Bx(N0xS74Koo9n`)$R#M0@vc4?*R*vA@vG;vXo$RMrym^=s?4vL z!x2oembFe5+tc&QG$A1sMo(v6G0d<=b7veLN*`Wj4ZkNiY6>fx-2 zb{1loCwbgt6s9mB#^7S$VRuR}nDTqTd89RlONo;3wfu&VrD7xBq19wggu;j!p@Jg& zkIa;+6j{~C(N7k=-V=@m1?8brlxjZg9jzoiTt}oEifD0PN69?fDBiH=m!{ix?-l5e zVpQui$fS-p0J{)IQTe&=%%+@?8?ypPomJ7X5E?wku4vpY0UkhSI<>2->%7n)h{l>B z!vwe~+=I>U%By)>-Mv=P)wLA{-Ecp78|5zFcSv?*J?yu@pJP9}oDm*#`mM%+Cb=>? z$CQzy$%}r)e4!LN_ik9UnArV3^=sSxA$}jF*QHg4s*!p!?p;!4osEv^pA!MVV}-^7 z#okK33xit(2`PhJ-P*lOw5mm&f3zdLpKAg8q}LkBuX&j6_qDEHlu|ww+%Swc0OP zTs);G9gSv53EdK8FGBs{KAFBO0oYD2UR?TAI4xGKX=jw=v7S)L)CkHtgeEGlovaa^ z1`QbYX`I_Xvb~9m&wF>V%!^fbbsxRMThzP9N6&{_%m#t*W;&_?SA_r@9OEmXA$f3; zsLmV4OgZ%t1Vy}vket4eay>N5$>Ya0dO;St{Q%;`J(v$1RGBn{=1bdu@W zy{T~oI`ij6Onv3>&NsT%YT*pnYaXo$+B{k-?waZ~u#_uMVqnYu-kzBMK^^ARsCw-CY(S-Q6PHvFT7$ zP$ZxrbDG$y5XD0_xH!=buRG8X7BZ^m^Cx^J@px~Rceb3W>G`jm1_iFvNl{_9 zbzjrek0?Vqj-zrtsFS;gcNQ1dx8khM-#+uUR8bi&yXiuq{Ocpj3D#05Cy&Y!d5T;> z8c5Io<{WHn>6KJf^oXS2uf%A5p_Ayr57L!$~&=w=O^2f!YsOV z>T23{EguK5u`VtsetVX+d}PCG=~G^w@XogF#;?L_Sz7DrIa^g!qg&*g68Cy)FP>~2 zj`1HdsfF4nN>U6J1PW&1->FGIwNm>qIcwt{pcOz>9e(yYh(6LJhy8&>1vdH)9Ye8t zHBYAbVGp?`LB{Yc#pOzPP zDjzarbZE~n;|Go4)T#b+1*Vurb9tvcNw|1EwDxWupEj#hICh|)h}RcXXg-?m>+cDr z7>t%0Z>~F%K7%g9*ze@JIAr`WkJZ zGoy0Sb$h~Vxb}jN*X1K}!O3OrR>^on`J1-<*L7N7@@eM9e(asUCz9WuaB)m4h)$@L zHYA@7W7xFtr)s6fvBQP zUbi&{u(i#tN#6 z0vW5-ELH6aYj^~MniHw*X=$(Uz|rnXTE!`(#+DEY zfE(r#mZ6e7^?p{a>6?VH=S`a7$?t>C_EeK{@%9`jp22E2?FFs3Uz5~kpJ;XxDE}23 zH1Z(3gBC}5(uNpce)FOR{mNlq60=|A?7T(=E*h6*_5QjEp>;!+DNpEpAehbt*-WeNY$` z>a5dQ5C5Swc53A5)OvN_h+*U1CF-7gXJc1-Ri!NSa)JwLDwkhNg*NAGESmT zq`Q9Z!sI8usSD(2Z;WAzAlYQ>jXt5rCOQsQj=f+0!1vx zCmOxI(QWppjpzI4%IfO8mgAS+c=3N#(oLyb5_+dPcl44saBA55IRLN|;NPTI;eaqTT*HW!3UU(NE#qtmIXiZ0i zPGUJsWGl@oC+t7t;NbXrXaSS3W-d~zMy3{$f2dzy-#(nagLT8#v!BH~n_w(bgtzGS z!__n+%}PNP>I;>jYCHES(^sl)%Cw!HI^P>lc>2k+&O+w5hLpXsZE$VDyE?Aq(5ofN zch*&dhWLqub#tHOQTKX&=xAV*j&jSJBndA&dhVPNA4VKpQjyh3J{Dh4V=`>kgxN0J zDP%M{XP48WXjfcj+Mbo2$c;T5?a(1#zY|UKK~^HKAXE5C{Y@bs=};e;5DF8b8@V}# z-f?kPZkdnfx-}>_6s=iegk#2?Ru?iE_ET6>L@g3q!F2`!li)rv}EGb(i^QKP#r zg4(s$xC-0X)@-`i{M-vGGg@aSUZV_Ma) zGS!X2DqJct-8@cwPLXY|Rec3ue)a4AVKjzg7U=pr6&L5>I~TyGCPazx!k%MV8gAoEO;*WP6^tJpYYM7>~43F18uAdvl9Eg+ht4%t7oz0izOw>@_sE8bJ ziKLF{Zrm6fU**Y&3k}%kH;I#VYt=A;|P+aXVk-;BE$oJlNaxnLi}cYj7>ky}=( zH8#wYeV+`kHmJEbJuaQvdgkL&y-g6`CXF5HZ4Qag-|y#=g%Z_0x91P|0^E&)_-++m zJd=MbQ4K>O`yDLRpQ$*|H%Cn4%jX+8U{%MUC~b`zr?*Ia$tKbHZFS>y)F-{fSKQ9m z|Gy>Dn_o09@yB%NLh zDG01q9{{S=b&OE^6#zsjJZvY}L@QSwU1-u=&9{ZFn%HfCSHg0CwPxq`~HdPN1o zz4+)lhi^ee>^PTH!ilqL^@^zZ!c~8!>ww1Pq+U*Lr6*2>raiBYW)YRbX6X^{H>W44 zHm2$Y-t=U-a!h$H-?GvJ(@y)U*@jVu4OGwDgtVb#^L@4K%&Dw5R_$z^lpE~miQ-ch ze%=ZXv`TJGg=~XT1?T9);fYJGQ7*G&|Up)kn z^g6apm)(E|f*Bck+&GN}eugIksYiigcmYV#@M&jU#A1E_?LAGWcnzey-xNRmKX8>^ zz=?m&@R%pc~knG&=bEfKY;-@Wq|B3t;gaP5ESOF2TZG+A%_@pTe{7mhr`oF=*Zxd4j_z@0FI&uc9Q%~OxQPVmi%F425_{Lj_J z#l@x6xx(67_V~c8&0R~Td++5PfZ13xSu>H(d~Lb)ojcw@bS@tZ##`zL;Mebe%T@kyetQ#`cqZW}E69HlI`%annEfk#MjK*j0% zYXq~7{6zmuEy}i@5PY?Be)LSYr_>ccc z2v}dgT)!K@$<3XckwI;F3^1g5YXC_s%riu+_uVM&-n~25@wT|rp_e&gilZOqu=hWo91H7ZsPNgrjBe9QL&G$mCjF7UySgq0Bl@plKt~;F4fu1* z$g6)%{Ev08K+-rpZ51C9WIGF9=k@WalKi57AD*VohkW)8mVX2TeKLNBdFc`T^x|TL zf`S5rsOp`5spcz`#qj@y_mLNh{iE+-vHHU9;1b%g z?&>&(yn6M@tzIO`6#*%^KuAU=QBWg{Mh2(8#%tZ>JOb@u6y0;nSCN(YhWqdjXd*;t z7gLz>a#-Lcjchz|OGjJEP_l=(C(ac(ezUeUvx#$ylZB&MHTv-r93<11i5+AyxvQg( zh}Lq#4Gkmw)%*^Ff(;=59kKVM*h8tV!!7p-or74-BiM8-$n!r@9M}7vB&imvXJ&um>>IO)!0enYFDGhQ8Q5bo$k4^zqFT#uZbkU=+Ine$dL@1&EuE1}tYTEvYl&QU z#-}++cXEwCo4k;`!m19Db3~4*Nr?ZRYR536L?piRk#fTjUw;d3!jPts`yJDjaVrY) zJjpp03H(j4OFx@us$ns*kO{|mDtz*{*=w4IhbP#v{Xe8F63&%(@Za;228l2lv#~O> zlv^1q-pw+l`P=BYd9XD%NRgu|R%c-b?vug05TK$y>cAwKXkN^9VbswGj?7_xtl>~6 z>G`@eG?L1+DV|q@tzz|!o}UGS%I5eZqsh>$)fIfy%1JW)K13a{Vs1CFZVZmMbTDnU zm!#jRJ)bv>%J>vXonuAY+jf2vEOLJpMD=gjUO)IP-gcZx!++6k9ifw6xXL(?DKj8? zSBJBo`ewA%I1C2N@(bBMeJ+JmuO3RWIqv&!=GDC z^JY6RPw5{r&dF~m;OH-|Q9ctoxy-{_aS-mjEofA4ZoxQZ_UbBj=)mYgFZprcBO@!i zklx=a^A+7&K?#!=0gm}43)hX&ZF+lK-5!pWuV=b)y4gjN zo3oZvS?TH7H8pPXT9sDg|+n%V5w}5O->UQk8UX1wH`jQNVwWmc!}?Yp}n zg})ighv;mKf+mhXM+~;zyuVVzNfe$TiiX|N_Yb9PhLbM*nfRll^f6%Sg8fFzU?hg% zz~6V<zVHKRl49NZ$dhbpFmV6{B!y+3ain5$VTA2s%sy}+S3m@(AC6@@LUXgI zM?h}yyj8TcNQ(oTe=qW=^Ti49$R-MsII-@@2_I5f9!EzV-=*4>`h=iv*Ooq$+X?>9T1gW-R_Q)2}NXqtM{mOZ;g>!KnO`w8CDz z21qo0v|gl_uzrz2YGo0;{M#2pz$xNg=F$O<=p^=+w{?h-%O3u8rlG2}&YOtn&K zrIjU{fb-!o(u1Wj$0Hgklqqf2ZhLvYy(ee)W^q0ca zL5|eGRHrxrL;MxX11w@L8m;E(SwH2uKiPMDbmWHkDa{S1u*fTb{SAUGe&l-}u8)*` zLo@AbFQSYzQm|enYiem-B`iTSqyEp!ap^%Ng1LplzCz@!inbfUFw?OWySEL%dT|2p zMF)jZnOuKRy;{=QQ++=GfA>R&R|)sd{@l9CN;tkdH$%znWOU*CIbpUV0@f>m)rkRs zv`Q%TSX2;B(kcTBqm*_2EC)hZTXRdx8>B&X(73C)qWd#*%}X^MR77FjmgK*Wm~uXe zx^;;XvE`6P{7_i+$NxgPh{JpUkoeE!Hb5S*^4!VO8#q%Axl3Vk!U~?Ao~B(9Nawl~ ztZg9Rd;yneqS}GWd3~bqm;qh46<_fw#5iv&=a~w3rFVcG!)DJxsorF~GIe+F{a*<* z6+UgJTRh_>WX#n^_pF3h|6IV3SI#C5%p^w719q;(X_qP06SDfU{2dP2eqIiyKfGzs z9*9R8<+8hZbM}l5Y zRnFNpB!Y+Wj^4i7+vGo_t)08KxaL^!^m0%_%YI5Tt0&NDi7gJM$+3RbZNp}Cs#Qw2 z;Kesvedn-Qj`n1FMQjhuTXVR}pEtBpe`c^62EGuJinpvU8 zr7HIpr`WAsgK&nb+}9I4j;}}TyM##I9HQ$h4k7l_mI{q%pc9Yj{I(Vk{DN0RLOSWG z3wc~~4fNz*oVt5I*^SPrbUs|YSYj9t5O8L{-%PC#f48t^$~WUS=|R}0eW1R za-7yy20NA55}wTVf#N4gjX8^P?Q?_tb4eD9G@&4;|sTR#+>vt*EOUs=1# zGL#lBj-%qD=W!nFEud16YE@0|*6<9|R-;ENkyjdi`XqG16Qx^uMY&!;tyqcb{x|c@O zw>mf(;*vA{tnrha+t66+9;I*S9pHN?pP3)nvQ@s9y<4G8l#eGO7JBOR$^Jf*e0ziD zp=En0(|F`-zB!H1{4$wQNxlBln$lpk+`n&89cNZiGp57zJk%O3xqAtz6yB?$@WLuc!z8yGAb^ejrDB2W8_0`(;v3QbZDUWtuhAcL&u@9TmNTZk#&#byb8lj@ybI zQdY*(4W9GR0+6#w-!}bbldBD+E8hucfhO<$rIKFvCjCC7iWhlRW^(dZ=-JZ#@3)8b zyz9XdOZlF-djN$dZrr-I``-`Oof|oTlaqe)f}0!lK-DY$-k9?AuLpishD@3BpQvVH z8E2~4CJB|vxXAC= z;Y=dg8NXuz9hZ#I@?Q#i?ie1vnsSH2$Ez2Go1tPd26I3M>v&pR2or(UbZ&tSY%$4P*lIah!k#@Wg4 zZx?+alaZO?DI|l+Fy6*DIp$`ZaV^T-M~#afYKmuD{pm?;%WOHwyDqI5Wzk=pS+_5# z87uwVs8Aw)^wbS6D}}0H&0S#BuzznbI+Md{+{}fojjCt4N_UcGZGW<{$uFs$b7?<4 zZpR%zuy%&GtxEdO*v{V463yvv#Q}&7Xf32SBMAva@>cen*r&3Nc=wt7WH#@?v-1X+ zpbF^^+HVK3KFr*h_MoLLa2~>2)Y3V*0pQgNO1RX zO*sJq5sezh90@r&9@vrd8Bx0IQ0YRkG$|(XpG<2ObaIW0(sgS`(o#D+j)-a|hhb~L z!_yu)5FbGL($3#)Jow-WeEe%q#nqb$bPZ8_B0-orPp90(&&_f!fs|scnpLC9AC}0h zb9&e|gZ4CO+@+0;hJzFvbLi=~=ZGc+;tHtwGEZ_|%pmEgBzhh$@srl`CIh7g>f+J? z{#HZo{TtT;*ydm7NL)j$YSNtj^x|hct(8tSBH1ALJ$5LwbT6uyY}42QwLv+eSU>vR z`8_|IY*<-Vee>Sy2s$P@p1EGs5>;e?`}5*{5ty~$*JnqU z2B2hW=tM@{PyLc!uCA6jxyV@S9Pl5Z3dc!uS2m;rM?G|o*{Y38bNB`ft8Y5Rd=J{@ z;Vaf!98#K2kvR6U+J4_-IMSD{gQ0PrPH7Lpp0PS_5E$M&s~*_VmRuCDWP)^Sv(>CL z%+;D!C>^e|%pqR%pM?&YLyje@=_s$~VOJnd!>I0$*%Qpi8R3aw4!2ukrSjd!4 zJoU4%?M@IZesu;s>1e(4qv3g=wQ~X&k=0?LhrA@|CX%`DhI)A;NZNM+zf;W3xtHN8 zcU4Wo=a=5My?V?Y#`vcU|$vw{DoOM+vr-kniKNRMxgQqg=#_$;&nPq(+3>vp6y+nhywLYtec zF}mkxF7v`SwoW5}cyLT}7Gv0yGg-%mYA?FlR=Y_M!?r<#IAR(lx~_ijF=&fxKaXr!#lxINVWvOyU_Zr=x09L zq|V2!M*VsLwh0M2>@f}kXxD}1MCV7i-}@|0UKTbkclAx)84AT75O6bNy-YqV88DDu zPwYy1@tRC*q6C?|X@8NB@F4RSb)<6EOBEel8SjAuU`%_915}$_9dg%A>FUdr+ zJ_Wgo{uc`s4N5cbe0X;7Nt6N!NK26$L^TnI$Fc&@`%@IIX6JKO!Q+3|fcP ztQ6Kdp6(8>gi0}!WC~qm4fj2+J%w(u<{)LP2{08@&7>nl-G>jKA|-b4kx>3$Z!pF& z(v)eP#ANYbtIHyFawt=Eu5~1#dtPXy=#;O!Ri>gzg*UHe{_0wN+emS|UBWw>UP&bd z3FC!m@`tjJuN7{G6Rj`=;3(I$FqrgiDv{~B-i02# z7?vdTLiYU%`U)$R7CMPxD!R4#zr&wQTvN5lSJtaH{(q87VJUGi{vDJq>)X4MDx+i# zW-p$XB|NM?t0@682N#>H9r<+5P;#s+uG7el0e+1zBOmiMha0p4bN*J|)Ay8>(4U=) z;sHrpSkQ9#P+@IxoTv`29+}+_f~@}gFQaNbDX@<#=uNygX81Y(IUzT%_*}&b;77!DLhoB-`w`@Jxc+yVa6y%Oh@GFN|u==H%7nQy-$&hIcvmi zt)j$TV-JU={n9I%(C@mRO6a&{yd$FbYG9hMXO>*lHMu)ff{huZ$=PDkDvTqTTe6?P zKJHM)Fq&oRMaa$5^~;e>Z!%N(pmGI(BK2QjGjYkCp7dT7&#F5^Ysg0*9_m7ZG6+dficMUl@JNQf*~`D0-W@?S zXhY6tKbt?iTVUfUg`8-RqIr%+GT{d0Sgmm+9LA=QjD?~QGt)81Hl%>dD6|6wdS4K# z$AtoTNH$fDzWqPn{H{ zpYrahG`mqh;$kGMRM?d`Acx8MQz;7jP@PC;qmIOuN zQ56k8@9bRG%TZRbP~3mx{P>@-r9`gGY)Hzce3?bcvUk$T`4P{5@lFt;IFnFf&`ala4gU_EENV_NoRgJBkJD#`exLd4Z5@vV>jj9Ljb9HZ z3>5j3v=K5M>d_qvTfHC(zj=c0_6;YeNP+oU0SXI@HQb4{Et+Q{?#}nsR1(hZP70FQ zty5k-GWXH+my$5^H@fiQj%>!`jH-ZNiecO`2Ab8cIc{{PSL8-W^q^nJ^!NAEkiTvB zZ0@+sK;UozpQr;@NuW_AfFa8LX=LrUl|z>a@py;c{o~UK_nvx;>-4seu@v7`_6X0V z{V73){uxY8U>)`&A`$2K1TVDBxE;OwiN=r73{eLY^F5!rj-9~4>|ze04I$;3yS4hZ ziZ9L*=VG@z$f&*InbA{eoYtCku_7Xx4wA7Y`Hv(8lpN@uFV&(O#EoKdiwyNcHhSZw zjpn7cs*mcJguxS1riNw3s_pFVR?*aqh05n-q@GDgsD8rp;P^Q11Lb6`vl>(&ugNoE zsqcgM|5GT4Qr(!Ums#;ToVyD(tCNf``5YJfxQ@pV0>|oT8k|F&P(m`Y{JuW<%U7@F zftrQ%f|2Nx;U`@C(rhxj%3q*}`i~N?isajp&lAs6k{_Ipr39eSv3GrG=)d5s3MI%V z`vwKdpG3|9(4sg`O;2fDf_qv2kOWS6gV4;rJe7P}tC)J-XW5+kOAEb(Q+ioX4y%cHo4o1t zTpRW1RKCO&tV)&CdB0~G@rdsr_ogvnFB3HI$*hY>tt(99oI8~c#HqJWipPvIC?$Ck zk!coa^veD0nw7qMBPMlpT#YU@AB)940upjtPh|gJ>3xmIp?%Ew@!??%%ykVaEI7EgoD}=bJ1)x+`TYrp z;L$-{TG@D3nI#qC^w&m45o2;ck5gS-Tpn|BD%ZR1vVVB|{(GnoCE>h)_@@v1x+6)c zGJIh~gC&EiL{SL^mJ$~-tY&uo@@V&k>M=jY&!Vgk!~*-jnsr9FDASLGq<_b?i*?O{ z@kilLPiG|G{)EGvbE6j^(Yi9ZTBo}&&w{E3ziWkDbKo@JmZx54=_{zd-VMG`6r5zj2#cUAI#@5 zhR1gFs1phLk+5t9g_-ijHc!96 zg2)j;1}^V24^eUP&f&@(rT0-RdP7iZuXhSfh6aHC$>QzpEd;_ilG4(HznlHYcj&dh zs70ry`NwB7j-<~uP6yN~N^fu7otiz^U*9)*Jfqpz+Tb|FnGhu?K;JhelxkJr^}ZGN z+%s8X7^S{x54_V)4s75xoLYo1kH#-vnivPw#Q@h~}N&vE0=p&%#(2L_SLT?XyNF z$*15eX5wUqTHc7d>cSoBoHngD7bBy2W+vOd=Kq-YOG?+4_Cx((J9@j5CHYj}DG|-j zdv_n}yQD@|OX!E5FPZL=vWeIBxG?;Qz{uW|PM_am!oOp(F2QoB%k_boC`K|*CmA~- znhzW67akIbL%)~}U8J|7|HqH=;8=6H?U*v@)F^}OMn;9jNKp#-)WWbxhzJ3KmVg2W zm(!{`C^xh|#iZqq);goF+#pT{^HC};O0+=>I~@t@aT&x=__uB)E2N3hLGRbm(a|GF zUhK}t*_eNQbD?IvW(BmSL2@KRD$0Ck@u&HCMFEKF>;Uy7;;?l`#V_jH{a~eNy5Yz2 z9TR+G+P}?=amp-Op9IP_xYcY9{nAyVt!|f7=m@fK3`s(N^PX`<;VDJe7rN2Wn6C1E zIQ2?fj}Im&Uo~xrWKqxDz8TQ}Rnh#vMKV)2Prmlj4#%6m0hO&5lDW9+Y`-xtb+;7J zCi4ttnj(kBHvP+TY79cP)sOUKe>QbFcAq=Qe+s+7SHAbE4qHH1Xequ?^Am-(KwYPc zg->YE9quhmLBe&LsRVR5k8#v*-X4Yi@7FYS3vc}po>5*sVCY+Jl&S7Vo4k2sNLk31 z5{^rlpq7@(!_-)&u}MIHzLY(6`ySW#%dhVDKG|KizCU5bujCeXa)PbYM|^9n*LUC^ zHkLfsCR!5fNwp;~5I`yg1GFa)A30(#&(W!^fY!MgAQ|%mkd+#=rxVGsdi7kp;g?%{% zN&5yz8c~N#8TqtcQnSU5ez_O!yX#r38={(tEKAb!f)j0nc&}W&%H@0i<_@R*5R&#C0@uU2UTDq}v@lc%bt*uMcc(A`| zpjH!Z<(BQ^e}%kpdyC$FZX||%BJIamv>&(GK+5!a;q=QXZvJw{}TSG%bv-s5q8qm+VFXR?J=5%_r8_R7K>+|j%5g}nZLd*aaUcB51 zk~+d2=}==4d6$mLCvhe>2D2~E#bq*xp!^@v&YPXo+2Js{;SUNZ65MaR_w_Y%Vn({6 zmtCXl8qJ(`=&XpcU2}o-sDp%2oLKHe9TA^#Hnt#j&Xd|ZROU*43imL__p;u5KH{?e zf#JVi(bfO;@1|y1TSnWe z>qz_NZ{=7kr9nR_d7nL|0 z;v$CiKO@!IDxWT3y&+m!T51iaQwsR{^~rO^nEMYOJ`@9SA*lDqGl>c|Uk2r;R*;oa zo;R$>50*{fCjo%cZm;t>7s6bkeE6{GziZd%w%JIDiA@0>Z+R&v=j?R27gE{&@#cR0 zQLK<>;IFLE08iIpOVjk;&)hy^qQo4PNtF%lhmMpDSy4GQ(-*b_-`y>+bf#+mxD)ZP z7t3(ae&u449$&}bvw8}&^=4}WelC9_Xa8UN31*Lq5YK`r17cr0w#knhs_I!$x4A2& z3vN)l$J}1iTaDh_;_|sqZ1i0wkjI!tn!6SE8aMWG=NFsw5XbK&pFOWV&2+0zizK10 z3u|nwQfYmhls9wfCb3a~JAF^IAgbhc(k+`V57$gK_jOCdGk1yBl8BW6nc7mR_1-Jx zHg5CF-ouv!iwnP(44)n@ZxZeBFt`_v*gb0hBKPI?p>iGD@hKJMQvGP_ESrIvxfzKS zV`+)|mg1j)C+Jt`Hi5qXNbj3S%SM}hmkZ=bZN7+WBS$io!V%~>6x7TPCwxx$Y%3uB zg6I@F@^)+2nrr659#fL#3&;Ohu{9y}t=~mY$-*)jEkxD-aau=FPgm@3=T#|Uc9^z? zBl6pu_B94a>50AE+}-3xmry~RH0OLHj)Hj(@x@DJ2E+@!qzf$f)PnS;^ETr46T$*f zA6RyGo*H60e~Nwaesf!%H>fV~$9`$xHi<-Q_VG*r>chrdi=(0aRUOYp{C+gq!stps z)n8M?hS6fTDRu7jA1hLif5%Z8R^0yWfYMe$JmBu@E#PTEY9VLHaEsrS=Ir!urI%2Js*O)78fM_S!@@{9~Y+ zo+qXXz~4YHDv?XX>%-`zO~{jx0R6LvdAg#9f-_Qx$nwDLj%$6xQ?Xekj=*%TrPc;Y zSFx$3?bgFO%b8z=sMZpmKLI}q8+@HB?ZgztaGGF?nHzPA#jZROzQ z9LiQ0SNRZ)MQK0PfG@k1#4~it_siUBw>cn&WT>K(BNCSdAM5Z1p}nu z4^0nu3<+1iWYX24)#H1rJF$ck(_VPAJ3(S2^paITR?_9*J;`RAl$&=C>G9O(iW{8% zjT8yuB%K{D(!BQZTlc+PEn0Q>T=jS}vnp@*asBwua>>Z0{07ILVKPo`A+^;u{=_M< zk{5)+R@9Td$0HEmXGhfo%9L7vSDdH?%_2Y-9HY9alRvfw|tevj?T5koNmkW z0ZVhO(VO2o)8qfSr>P%&sqG@3(@RfO^vSQ|vJEP+xp?IU>y+2A)y%IZe6P`~yhwl7 zKuEp@f4!tK1|D@_<+Rs0F{}O?1+;Fx#r+QY5>z(YeH-05!w-MUj^PZl0fW|yLW&n+2UdLP7LtR-|U#w7n( ztL*kqj;|*#@?QU-?fcLD!PrNwL@VOEJR}@`;`eS(dN5UNZ{f0kOKm0DF7Ra(_0u$d zajbi2binoMfmBRhz`=#LzEN2b`?=$f4fpn5p_yzFZi`TT`){ifAK&Hh-qYV9t+Nr- z+E!ocd!n^>I+9BS1wG-|NiCx5%FoYq{ZM}^CK_~#ZYvzl~x9p=6~$TaLhuc)e}c z;dzq4)4w1_`c_mV4>rcHFEOo|--;{cxP5Sv_VGAV7w%k>_7b`Bu7K>_6uD2}$A>^F z{zK6|u++MGp+)4guGo`<*M^vl3n6rzITZTuTfUP@#C_ITbKFc0*;#mXT;=uVHXABp ziN5Te&HI?dee=5p-_ey@lV8C4H#WXxYG`WBb`>r3;w$Nj*6v?fAv1o_+Epod7Qpb( zTH7VWs~-KH|Lxh7ns8jR$-R~Q4+eWD+CYVzHwRGRIyHz2ju-G=6e~9-Ybzg7QgYe;dQxtV>3FI#rtvh$sbD&N=QO6Hy<185?JOoa zB=-0!J!^K9iETHjaa89gLKzo*ADl$Jf`j#ih=j1TlRCk_EQ9hcn8^u1) z<07`|32zbRKS{!gP?3Jit`udl8!1gRxUZsbEAj!g%|%XosD;mEqDCnyCK}1F+mxa+ zRwkfydG{jWh|tRmyWMR+MvEnuvf@^Temyyiw7)eE(RhG|&1EONFO|4AhfYrEBuC>@ zy5c>Een>T}`3y2}c8P5OVHSg^0x5Gq{5EiMxLxkD`3fWhGY74jo0~C>-rl<#(|TTB zUU<2;iW0(T<;-CvAuwQ26Tl{@Dg`0P0_|!=sJuHXfVW}|iiztP?o+t^B@Eb6MB3jXARmVHMhG&} zPl%#oV{;F;7c8I=F|{=?r3jSdw?N?e=UdE$ppb}&jHmCen1L8+hPAl9Pc9=!10YZA zPsHH(7mQ$~`(lb8Apt>}-+i|qZ!szK9p^6D*c7_yWayXF@$ z$xq)Qs(^XD1kuSgRW6&Ubb^P_GF5V79iCW4S~?&#Ee&3r8yemgw~|scxYz~pLG79y#>iHQmJ-SHeHT2P=hgM*^7{Mz2yx)9mA zAX_^(KTl6fD{j&guK;^#Ze=A8;n3`4LnL0>`2m+)#7i643d2yskdAIRl0?_<#ZoPO zqD_>Sm;VAmSuR9<%a90BE6^;D*Jjcu1*I%WSy@#upfQc-q+I6db#-<4NlB~*KFC7> zVfwqOQEF~&c$!WU4c*cVq19MoVZd{~4wkB|@F<%%*tM4!80(U*;C)iIm10>K0yV)m z=7{+1#7KXJ6qmz%7yic2TaezpewP_Z6f4fW5EYHNzy27MZ$YB;?UeVqrzgOuM&<+g z8XSSdY|kjg0_33u>6-S#)r?2SQ^Uh*Kv?t-3{-*RiTvYQ$~o@`--B9UH<0N>ZT=CB+Z_e7iwJ+!Y_^hG z4bfWm4joT5H$MlCY&y8+Mqy|ABFN_cN2G*tsX941WgKno@2e>*hl2^7=YIPHLPQva zW#yR#Cf8`!0oiOCrN(sp{A1RJi2T}{G_ZSbNHrk%Kic20DQ7XFzwpKp8 z1JxGeX&%bYH1<4=c9tC;DB{Y_SkjJa5fa zNb}7w%CO6_n zGwXFp=Z&cXShr=UIZ-{1VK?$WgKjJ1+eXUjkoIbBJRgT`3`IaguTGHyUMN!LFS~<0 zPZ2ekeA|ogFsiR!rM4VFC5!GkAlo2cCny9O#j@F}9rcvh7cVBTJ1HQUP^gBnE0Cj6XaTfm4vTAr2=H@4n#T zdw*oG$y*Qrd~G@14E8Q_D{n?1TZ3lbr8t|0pCGCI=Y2=v-|g)I$V1RioaLb#6o(6S z7*w8vlqkQ`s%i0AkidE$m=RC~Rm(Zc2$kzTKT%Qnajz$h$F)z>Pu zwi3aPYXq+9xq80b%a?snWi|KvH#`@Mc2zPcor*%Hs;f?YDPvzOYqCN^5e){K7IOBRjw+c4VWet#p?~n>jQ9}Eb*gX_FreIOGYBzSEm3s z=R##&D~)0DN&cF z&E`e6j{+38^+BnaQSli_j@PVJj8jR-3GIeMIor}=KVu!FDE#6CY2dg(4)_Z0yCj%W;}tmeg`l~uoVx*6n}pi8F!B4Y=@U(#7(CByCaM&G zNU!+v<;PJ61d;*&L}-EV3bG(XNa;TFBI0nsIx`JX2z#ibR)!*|KrPt}3RHra3toWu zduR{jFEg#IP&fU-uq>4i>vCxExct?xow9f&C^+%wj2#N}^8aGEvxzN^r$|%_wR5tP zyv9b2lo>#exu{B90qj1qi`MWJlbEw`5I}e1)4FDVPRq98be&<{EpeuT%+`?eC~Vs zh?Z|D3bu?U=rv{>H$;w%sDCDygNsuE!sf{~-(AlQX}HPhkEfm@F2K(;ajkr~UfxZ% z$;!xZX-zeHH-fsbN$FW9L44KLeq;Hs*ArEC5jFcn$V~Afy0ul&)eZ~DWof*E0O)`J z+72KdMP~wMSfS^mUe&h}zt!PBVP_gH&{9JR4`q&+KZ6CGggfvKSJRpQhP&F=2Il}+ z;1!_oa%okXW9SbL4n{mzl2)=7<7*5S==eKbVW@3OdG<0XudD;ww zJbNAkB^V2vqaW+J4w@9!!%GMGhcguC*>HejB9mO_Nf3ArsVu8L#=*rc zuW0}omm|1>FyMrJUYM9W_J$5l%hQdGOVz;olGUoTR)LL?pz^N!Z!(V{rr-%qt zn;o{2+6)dAXkoGyC`;F#J%6s_#Gh=E=qkq2cv3RTucd47DiCgphO;A!JX+#4CSKl= zRo^~X@1G!lz5bU!_w(9#<;vc;jk6PpYVpRJ1(+;^Y}PfjHd5TY>VHLCz2x;L-y0;a zG_8N8e$dHGq^IMx%i1_EDq<})>joLn#>L%XJ!dg9WWME%r6CK<{76V>@=t36Xla-J zCtXrns7{}GCOWby=J^FcqRrc1WY9dAtHLl~58E9^mY^!9qIQy=&tNb; zuD02izKFRW1VjT?zW^mc&&a|e2ipv#t`FBf!3RITanJZr7k789ld(z6euw~rm*NnnXk%>#&VGqH z4`7S5%JwZl_EZ6BP`>~bPF}m+nY_QpjoDx@F_wpL0#L6d{!Okm;>%F5y=7vwsw?V z4aieiShlFd9!SvHivEl{m88})`-4XtMBYBy$QzYTYF*W28RPkyV*PJJ&GW$ZY(ceH z?JC=*S2fM4P+FD-gh|ZDTs5kk;9`_(cRT$gH-(;P1s2= z1Fn?EPg=t$)~A}gNv@Ly&Z;Ku(doi5HKK1G>^gaU*8JM2}^*==!U~LPgiqs$t@o_z_t}sACd8Gd}U~j%)m*{EEN})Tk(ZhCP z^Qx67dd8e-Zv`DW;!&kKTu4P%p{OP-%$_Zho zsP^7>OlSq(VT*@2SmRSXy=0}`r&ins)>aGNqvP|oy*&)7KM!ZsQ>}pqKPw|H5+C6S zqi_uM73e`eS`vR30e1o<>LaevAXZ)`pXqZyvI=Q=6RX8i^~ge0i~|GY8-c zmpDf4I8Np{@6P#66OH{QW#vRo#VCxWsL~LPwW@(ugZ1x`p>)N0x&_PmXpvb%OeN@! zo_orxF8t}glnq#rGRA;)fUFRkD{yF_%Tk2NNn?YWxh-ltTeSNf`y+vzUTtm<_&h!yY`ovM7IAn= zSI6VsRuXPE9_1l7;s700MoM!$j>+yiUFK=qoS9@`x>g0?X`9i4dGj3pwT9b+QIWsR z6ILfM*iQBM@p?dd+oWL5;|J0~0ZQ2yE2f=E{Nv$Dc1QBI;wa5ax9*Si|5;Iy_6cJ;UKRgnUUwJP%Ik{Ky1(gA(D8=DKR*j%wU3uWnicQ-FKYr8#B)ra{q6r=! z))3BW@9vHQ-VKPqit}xoBG1^?#XUOm*a5zLyU_$eRK(ylXFG#;UD%KGg*2nddWcoj zT){v7(y&Nif7~Y(1HGdBjD>nR)o;lQTs6Il))PF+2}O*m#&7N$D|nxJ!e<+tgO`*B zJ*7hK7KLx%_qUqAum+74X|kpJ-8))ZRd9x7>0X|mXqposBAJ^W9WCL-#;Fm%sHmuk z&)s{C>_rwYeu4r4VP)pIh-VDp&TV!hN2R^Jy-TKU7DZko907c;mq_l@U?jG9-7V39 zgL>IGmW}8WrbR9+K&o{&dfIN0Zbb=b&pUTyF7GWqaC5(znHgOU!L99JP>#!_QBKKe z#K-@FuNz{2%<%A{qbb{A2vgXwlzZt+JOc;!uzSLR!iDtove5|-J2durH`1Pry*M`E zQX2xbw6sd)UxE2xkM(5)K2(sGAA%X@04jA!y`BZFKh2_4w1%L-5O$4R?(Lo@pYkt@ z_g)<%=WpQAj9e>MoKZa%(}f}xh+uM#mQrgBoU1M6Dr2s=mEmB)X!xMzPNxOSxlEoz zAqJ zHhD+z0=rV~ZP#C22WF#@ULdtKN?{{Y{az7YKl6N8!$mMNAjtK2ZmZGKM|{Hu4m@pr za~)T4hEl;KZ1vfiCHHTT_FXhDnc%drX^wkF1V?Rkmn+qcb%av}t+Sn}u2Dqawb8rUgxtx*H*;CpQuH z(Vk@7rkY^VNzihc3#MM#&(iFDWf&HWOIG7SWo16TP8&3*l*9B~m|cwb2cygHE7!nw zbDCX!>k<5sMGuGEarmbnCl7%qzF;2;|5K`ec}O7K8q&+!!b@oQp{Ftz z$+I#u;cC1@*give3;&T-2=zVzVeuYxhIM^HFmcCG-g^OBy-Wg{KHe-s0-C7SSM2^~ zgPh9)>7`B*kJ4NRtm$wGf}Y9UX*YW;dDRir6{)Ol_&<-#i^Zotp2eYM~-)ME+{YDNrvT(oT2``Up?4-XdS znu?v>rfYL(I2pwdNZQt?pKDE^v!AD2`E~Kz?Hjj296Cfy3<_O+x2;@6{Pa+-MdBklc-x` zgN&#J@k|6^M-5+^{{;IQibTybSa_lpu(tebLaUyZE)^}NZbqsqDjEPReTupURHg^r zky7p8#=Vxrtn=_A-LlP%g zKTAF_)oAshpC=h7kBBkqLgxz}6JHPX^~HmSR~@!r7`XGC2 z3l*&eD`@{Me_QQijhdA7lk7}^Y6du*QYwl>qiTJI7mA~>aH-Q*f) zoNYm?H=)ed;sGH7ttvPe%sDidb8CB_!t8LeW97cK#5TL%cBiexz zmy+?BR=t~2LN~cB#3Z-_!|6We*T&p`^5ls_r}s`thy)G}gaUJMM4Ey|$m5MyYMAa7b( z`Vfs^>s94wGAz zy3DzA=K_c4G^GsZ3;zDQ5|tR5*KUb>^~U96xvZLBH&8E;F*nVuffOxz#=LjFsYEkN zjfTbF+Jde4BJ$iQm~?ekoh-^isYI#Vb4X( z5p&F3L-@})1 zajeI_`tc8Fwpq6%?W+C=(XrA_yn|7 z9>yhpRs^Ecz3-JtYNlpp$SENv5Bn$@ypnBAq<3SwJqLFp_w?cz=|sUl%J3Cr5MV^Y zA6h^EWf3{dV5y(;v`@F9mWCvx9PBX46UKik060xnk(7h7ww|8 z8!LYW^T;VHJL)w+e)$+w0{2un{l!g+I4ByhhaC$7<6WUAGv`Uz!J(d16lojM$T>Sb zWtJ>k7EQtf(-FwT#6(E9+l55E!OoY&5kHzx1ZcoW*W+)15oJTsw(S~h0sdsu`#OoN z9=98%U`|RyeW7zsWBHz+5UA5wzm<2Y9FlPBs+zyEITVq6ES_UeV;Tn=0OdinHt6&^ z5hz1Ir`J>7c^w&?VqzSuaA>$R(dQS?ME|GW%xKI2o3SKqW1@xU&b?&H_UQCprC^9! z=MiVo4Lt%`y?dh9?kWQRbt?PW6VM~ECI$H^M=oaKyKT_e4Aqh!p(ogXm!RqUc2RpC z?F%t6F(mQeNTqgiV2^O6H?d7VgN5Kvsdz3D|YoE>#i0Qzed_e5^X7?eZ zF&s=SbqQP$JH@0>!yIi9kh|*8PA1Us+AljeBO_y@JP#_ax#x^t>(8GCsg!0`vGZi3mqp$>kE3|+!ED|eiF&#Koj%-$!C~f@0V>4es4SC#o`w)B*6vlTgn7Bd{jjA zu#J`9zy;KaLQcS8SWXR)DV&z_&i0Uwj*cNzbkUAoh^u*_v#BdEJqQ_ZaM)MM)pH>( zh1Pw{V6hhSRSxdc_JkM@`WMuRl#mYNm=j2&RZrj<;qq3($0^$et{Z=R&;r9kz@{l& z2G@?oB91pmd9manHez~hAJj%OVO5A4XTMyi{{}8(su?0S^N1!Kk6OD5TA9?s0|;r) zdC(7Pw;oL)fO5=EH z3pyl{CI0>QzaO;+vO7Ah#6F|NvRioC5i(hLE|+tL)Xq=0YJiHfQ9=dDBwB&UZq-s> zykyDg*6FmfLWL!89#P<&$%Ym)ni0l$wDeTI>aSgJbE<#=>mmc(A{0wx{4cuD99{;Us5U`1oVG4@pb zehY)0tzmiz2btug;|V8)4+nUKl61^s2<8T4&q3dkM{zd0W=;8d9RZ<(7USD+F@l_Fh0Cu#3Zp zPQno40?{A&gDxrMy>!feCEnHayB$HBs^S`?|L=m>n^Y> zDHkH5J!h+#9w$0t?%{kGf8k*={VS$_L zjpMS;&d%7V*sdf-95tFT_viIbec`vx0IL!@-MDUJMP866xdRHx;Gd$Qv=PH;e?pq( zG;}R2IP2JD2;5q~BvVIGgfmO~InbO$Yn-|9M)fzj&sx3?2we-p&uVKqN$oera(Iz} z4llpG{66GrzlODSG*FD4K6AZ7UAo@pZamE@e1bThn!WBo;lhG?LjQuOyXf93OA$_{tizES!?Hum%8@so^a3?F0tUU^UVs z{QN(_vP=7GaKvNNcf7jzK!hyz`rBz@3h#l*Vzs5CJeQ5=6S$@Wpde!$3Y2C%cydh@ z*R)gLzS3JtE!Gc!-Dv(H5a}koe4{j53*ke;2Yj%2UWeM!g*-TIs(5xyw$y@CgN+Fj5K^Z5 zmn;cgGG7aNaL&bH_t7C6;1D&+IlCZ+j&O@-_(Bbch3EP(^wy*_dUUgKb~xO`gJqs8 z6`ZvJsCKN*fbY~DFninCY5p;Z`KEralpm0A!$IUgTL)0tB7Fip{|`B<_gDRh8c=RA zmHPNB5Rl|{{lVd3>FxS9xGRvkT496Jlry0Y#F+gF2k#5vAY%|%*qkk#2T1yN1G7dL zb&FLDQcw$kS{qWZbVzAQN%D%wm5^boJKw#1D}i(*RDGAYAO?VH>^_cW=BY8nK#k^v zyE{8MX=iWNHo0j=89!e@O#Fng11+=zJ;^&FbZH&qEbpz~7Sb(6XVc=B5JHDu2q*|u z%IsYC?Yp*MwW6IfO}Z%Q21(VO-QC@2@m)}+^~?6N&7T7PP_6np5|p%W!lDFe8JWb| zufv_b>zD^^xt5R)G>v`G4bwl3K}x%%3Yrp&Dwb)|E%x_^57nST!-1Y-p4&r$Du4E% zqrC(G-tHvH)<*?rH+ntMdrFWK9$OFRC{;h_q_Qcfv72y?{T+0Fu)`kRv@v&$D@sMgE7DTnn5kA-T%~ih&}HE! z^?3OpF%1$zBY+xpSFc*N%Gb~xf$GJ2H$ZoaMMj~X9=!uA8z`;kFphPpoA7rhXf5A2 z-`vL2O3_SQ-2Q@3_sP=JE!r}#s6h$0oV@$Whj0uo22`&9M1EbebmYtf!&*>sKm#5G zTaO*4@@O?!`HPM`=EK^{0GU`i%|LIO33t0tm~zkM{-Bgn$Q_u_6MrZ}4M1+l_f~%M z>HQu>Oo`LED2Z_9(SV=Eo9<7?7xJNSX!N+Ami%}E&Lw#K19Ro`Fto4{VPq5+MF+`U zJ_ct8{@l+2`AZyky`d~RT(oz5NGr?M4vcO;s=Zxtb}oUEf$=4eURHX{fwrSmsDbcX zF0YfhXlNWQpC5fiSFDIlO-;R+hPYCL@stFsU{yJhDNIDh{C0jO4dKh{|4*OL=w~Ej zLsoy7(}D#^W?W92!A7hT2J`VSt*ED`=@Ik~k5smx+B1|cOOo*K3{A!IkYbvC=}^nS zA>@AW&?(}nO#y;AF8Bp$7BiM0tW~dWZVFOLPN7EP>t$3hrPDKka9*O33Iui!v zLE~|(Z5zYH)YNU9rZ0(Mai@!!*)LqmESroG)Q6-3b)x)R^Oip$6$joiM!f*;p9l<5 znUi*d&G0wF=+&(qZNhxEjjf1nmo8gK8OskL-ID1f9@Rwzk@R*U?1t<615nHnb%mxZ8mYtEgi zsb6`We2|9`Wr1K%#T~e{;87h#0b93Lt?4w%8FazA1)5wbtH=!D>FNZ4E!J7_-Mbq= zO%D!Mf&Tn<#{^KN>7u*qHf)fyb79_Sx|i8l@k1BHs zhd-`I76fjyd4iObDDK7X;LEXywk-Kb77o^C>1rel~OMW@ZPSp9>7W z)yT29GW?TwJ2NVes}z2)&V~U=Se7Py9dP>eX@BXlu`#(QlorLyRD^j>(x3kMCk75Y z(n4>xnx~14ru^%v$HRnHtW57GS`J^NE4HCDHHrs0SzdP`f(hhU{X{L{U337t3rRN{ zbk@3p|F$f}<|HBPe(coM(wSxv&4;ke8$Li8P6GjctNud!vzg>hEcu@GOllRYdEwNe zpi&M85B^M{Zz;mzx2UdJ1rkNA6d*)=YTiSIr=DYk)2=~qAbcp)4zE9vA4KG{$(C?H z5rf~5g~@b@Z&sympI=`QEEiQUF`T$S%>B>1YZ?LJl&}@>Djc7BlA4Jp6_5`5D830q z zYA>fe$g83u@Ro)4c;0`VYQek0AU*bAEBf|N;wQ2b|moKAExQ4*_5$yt6lY>%2jUE30xI`lyS(>n%8EocyW|fXW zhK>N?&6#!!+W)}1JO;W!9EPI;|5yi0Bvq>n8%=!2 z{N=I-Db&1hV`^xO2k?-zMYqm^y_H1<&l|)*9C{Xe4oP$%tKlo)|({DbudvcinWq#=W z?>fkTnsn@IJG4*2sqSvk3dU9rI7{;Psh#_y#nd)pMgR5%h4@xYdHGOT+!npG4`B}R`k!2+=Q1B()M^*feXIVA zWxC4s3D+=bTJz84%b89ca(_Vd`D2KEJui zCYEi=8Za8m?I~z&5c23=c}JPX%Gnz5^lJCVD|}&=OOj2%s*|O0_m}6jhTxM;mn>7= zkce{AW@Ps|Imeuq>=T>0PPg_8oqkjmBHh{IVr$9A-&uv-TmHSJy+)`cL$77mzTED2 f{}&SyJ33?L!Qi1;#&53C#Z{D!9ZcA7`qO^^nMErM literal 0 HcmV?d00001 diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json index d978e5bcf7357..cbee3b5da72aa 100644 --- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -5,10 +5,12 @@ "covariance_estimation": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", - "default": false + "covariance_estimation_type": { + "type": "number", + "description": "2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation)", + "default": 0, + "minimum": 0, + "maximum": 3 }, "initial_pose_offset_model_x": { "type": "array", @@ -19,9 +21,20 @@ "type": "array", "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + }, + "temperature": { + "type": "number", + "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance", + "default": 0.1, + "exclusiveMinimum": 0 } }, - "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + + "required": [ + "covariance_estimation_type", + "initial_pose_offset_model_x", + "initial_pose_offset_model_y" + ], "additionalProperties": false } } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index ca486a320d9dc..6d34f666b997b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -21,6 +21,7 @@ #include #include +#include #include @@ -566,11 +567,17 @@ bool NDTScanMatcher::callback_sensor_points_main( std::array ndt_covariance = rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - - if (is_converged && param_.covariance.covariance_estimation.enable) { - const auto estimated_covariance = + if ( + param_.covariance.covariance_estimation.covariance_estimation_type != + CovarianceEstimationType::FIXED_VALUE) { + const Eigen::Matrix2d estimated_covariance_2d = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); - ndt_covariance = estimated_covariance; + const Eigen::Matrix2d estimated_covariance_2d_adj = + pclomp::adjust_diagonal_covariance(estimated_covariance_2d, ndt_result.pose, 0.0225, 0.0225); + ndt_covariance[0 + 6 * 0] = estimated_covariance_2d_adj(0, 0); + ndt_covariance[1 + 6 * 1] = estimated_covariance_2d_adj(1, 1); + ndt_covariance[1 + 6 * 0] = estimated_covariance_2d_adj(1, 0); + ndt_covariance[0 + 6 * 1] = estimated_covariance_2d_adj(0, 1); } // check distance_initial_to_result @@ -807,7 +814,7 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } -std::array NDTScanMatcher::estimate_covariance( +Eigen::Matrix2d NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) { @@ -819,19 +826,9 @@ std::array NDTScanMatcher::estimate_covariance( std::stringstream message; message << "Error in Eigen solver: " << e.what(); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - - return param_.covariance.output_pose_covariance; + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - // first result is added to mean - const int n = - static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; - const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); - Eigen::Vector2d mean = ndt_pose_2d; - std::vector ndt_pose_2d_vec; - ndt_pose_2d_vec.reserve(n); - ndt_pose_2d_vec.emplace_back(ndt_pose_2d); - geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; @@ -841,47 +838,45 @@ std::array NDTScanMatcher::estimate_covariance( multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); - // multiple searches - for (const auto & pose_offset : - param_.covariance.covariance_estimation.initial_pose_offset_model) { - const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; - - Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); - sub_initial_pose_matrix = ndt_result.pose; - sub_initial_pose_matrix(0, 3) += static_cast(rotated_pose_offset_2d.x()); - sub_initial_pose_matrix(1, 3) += static_cast(rotated_pose_offset_2d.y()); - - auto sub_output_cloud = std::make_shared>(); - ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix); - const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose; - - const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast(); - mean += sub_ndt_pose_2d; - ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); - - multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result)); - multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); - } - - // calculate the covariance matrix - mean /= n; - Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero(); - for (const auto & temp_ndt_pose_2d : ndt_pose_2d_vec) { - const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean; - pca_covariance += diff_2d * diff_2d.transpose(); + if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::LAPLACE_APPROXIMATION) { + return pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_covariance_estimation = + estimate_xy_covariance_by_multi_ndt(ndt_result, ndt_ptr_, poses_to_search); + for (size_t i = 0; i < result_of_multi_ndt_covariance_estimation.ndt_initial_poses.size(); + i++) { + multi_ndt_result_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_results[i].pose)); + multi_initial_pose_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_initial_poses[i])); + } + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_covariance_estimation.covariance; + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT_SCORE) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation + result_of_multi_ndt_score_covariance_estimation = estimate_xy_covariance_by_multi_ndt_score( + ndt_result, ndt_ptr_, poses_to_search, param_.covariance.covariance_estimation.temperature); + for (const auto & sub_initial_pose_matrix : poses_to_search) { + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); + } + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_score_covariance_estimation.covariance; + } else { + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - pca_covariance /= (n - 1); // unbiased covariance - - std::array ndt_covariance = param_.covariance.output_pose_covariance; - ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); - ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); - ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); - ndt_covariance[1 + 6 * 1] += pca_covariance(1, 1); - - multi_ndt_pose_pub_->publish(multi_ndt_result_msg); - multi_initial_pose_pub_->publish(multi_initial_pose_msg); - - return ndt_covariance; } void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) From 5ebaed754d7a333ba46493dedf1a8a235f00c8c1 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Fri, 5 Jul 2024 17:39:37 +0900 Subject: [PATCH 15/33] feat(pointpainting_fusion): add test for painting util (#7169) * add: unit test for inside bbox func Signed-off-by: tzhong518 * style(pre-commit): autofix * fix: correct param name Signed-off-by: tzhong518 * add: comment Signed-off-by: tzhong518 --------- Signed-off-by: tzhong518 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 7 +++ .../pointpainting_fusion/node.hpp | 8 +++ .../src/pointpainting_fusion/node.cpp | 7 --- .../test/test_pointpainting_fusion.cpp | 56 +++++++++++++++++++ 4 files changed, 71 insertions(+), 7 deletions(-) create mode 100644 perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b6dfbaf402cc8..6be7defe422c3 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -172,6 +172,13 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_geometry test/test_geometry.cpp ) + # test needed cuda, tensorRT and cudnn + if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + ament_auto_add_gtest(test_pointpainting + test/test_pointpainting_fusion.cpp + ) + endif() + endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index a4e6f315a36d6..756cb4224bc20 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,6 +33,14 @@ namespace image_projection_based_fusion { using Label = autoware_perception_msgs::msg::ObjectClassification; +inline bool isInsideBbox( + float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) +{ + // z_c is scaling to normalize projection point + return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && + proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; +} + class PointPaintingFusionNode : public FusionNode { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index a7c83ac7614a8..0f3a50628ccbc 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -45,13 +45,6 @@ Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) namespace image_projection_based_fusion { -inline bool isInsideBbox( - float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) -{ - return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && - proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; -} - inline bool isVehicle(int label2d) { return label2d == autoware_perception_msgs::msg::ObjectClassification::CAR || diff --git a/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp new file mode 100644 index 0000000000000..1c69273667997 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp @@ -0,0 +1,56 @@ +// 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 + +#include + +sensor_msgs::msg::RegionOfInterest createRoi( + const int32_t x_offset, const int32_t y_offset, const int32_t width, const int32_t height) +{ + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = x_offset; + roi.y_offset = y_offset; + roi.width = width; + roi.height = height; + return roi; +} + +TEST(isInsideBboxTest, Inside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(25.0, 25.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Border) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(20.0, 30.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Outside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(15.0, 15.0, roi, 1.0); + EXPECT_FALSE(result); +} + +TEST(isInsideBboxTest, Zero) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(0, 0, 0, 0); + bool result = image_projection_based_fusion::isInsideBbox(0.0, 0.0, roi, 1.0); + EXPECT_TRUE(result); +} From 26e87ea80cca8032adf1d8261e4a69136ae1a523 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 5 Jul 2024 18:30:35 +0900 Subject: [PATCH 16/33] refactor(shape_estimation)!: fix namespace and directory structure (#7844) * chore: Update shape_estimation library and node names refactor: relocate headers refactor: add namespace refactor: update shape_estimation namespace on the user side Signed-off-by: Taekjin LEE * refactor: update namespace of the node refactor: update namespace of the node refactor: update namespace and directory structure for detection_by_tracker node Signed-off-by: Taekjin LEE * refactor: update namespace and directory structure for detection_by_tracker node Signed-off-by: Taekjin LEE * Update perception/detection_by_tracker/src/tracker/tracker_handler.hpp Co-authored-by: Yoshi Ri Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: Yoshi Ri --- .../detection_by_tracker/CMakeLists.txt | 14 +-- .../src/debugger/debugger.hpp | 2 - .../src/detection_by_tracker_node.cpp | 77 ++-------------- .../src/detection_by_tracker_node.hpp | 21 +---- .../src/tracker/tracker_handler.cpp | 88 +++++++++++++++++++ .../utils.cpp => tracker/tracker_handler.hpp} | 35 +++++--- .../detection_by_tracker/src/utils/utils.hpp | 15 +++- perception/shape_estimation/CMakeLists.txt | 26 +++--- perception/shape_estimation/README.md | 2 +- .../corrector/bicycle_corrector.hpp | 16 +++- .../corrector/bus_corrector.hpp | 16 ++-- .../corrector/car_corrector.hpp | 16 +++- .../shape_estimation/corrector/corrector.hpp | 27 ++++++ .../corrector/corrector_interface.hpp | 14 ++- .../corrector/no_corrector.hpp | 22 +++-- .../reference_shape_size_corrector.hpp | 17 ++-- .../corrector/trailer_corrector.hpp | 16 +++- .../corrector/truck_corrector.hpp | 16 +++- .../shape_estimation/corrector/utils.hpp | 13 ++- .../corrector/vehicle_corrector.hpp | 18 ++-- .../shape_estimation/filter/bus_filter.hpp | 15 +++- .../shape_estimation/filter/car_filter.hpp | 15 +++- .../shape_estimation/filter/filter.hpp | 25 ++++++ .../filter/filter_interface.hpp | 14 ++- .../shape_estimation/filter/no_filter.hpp | 23 +++-- .../filter/trailer_filter.hpp | 14 ++- .../shape_estimation/filter/truck_filter.hpp | 15 ++-- .../shape_estimation/filter/utils.hpp | 10 ++- .../shape_estimation/model/bounding_box.hpp | 18 ++-- .../shape_estimation/model/convex_hull.hpp | 16 +++- .../shape_estimation/model/cylinder.hpp | 16 +++- .../shape_estimation/model/model.hpp | 14 +-- .../model/model_interface.hpp | 14 ++- .../shape_estimation/shape_estimator.hpp | 10 ++- .../shape_estimation/corrector/corrector.hpp | 27 ------ .../shape_estimation/filter/filter.hpp | 25 ------ .../launch/shape_estimation.launch.xml | 2 +- .../lib/corrector/no_corrector.cpp | 9 +- .../shape_estimation/lib/corrector/utils.cpp | 16 ++-- .../lib/filter/bus_filter.cpp | 10 ++- .../lib/filter/car_filter.cpp | 9 +- .../shape_estimation/lib/filter/no_filter.cpp | 9 +- .../lib/filter/trailer_filter.cpp | 10 ++- .../lib/filter/truck_filter.cpp | 10 ++- .../shape_estimation/lib/filter/utils.cpp | 6 +- .../lib/model/bounding_box.cpp | 18 ++-- .../lib/model/convex_hull.cpp | 10 ++- .../shape_estimation/lib/model/cylinder.cpp | 10 ++- .../shape_estimation/lib/shape_estimator.cpp | 50 ++++++----- .../{node.cpp => shape_estimation_node.cpp} | 14 +-- .../{node.hpp => shape_estimation_node.hpp} | 12 ++- .../shape_estimation/test_shape_estimator.cpp | 34 +++---- 52 files changed, 609 insertions(+), 362 deletions(-) create mode 100644 perception/detection_by_tracker/src/tracker/tracker_handler.cpp rename perception/detection_by_tracker/src/{utils/utils.cpp => tracker/tracker_handler.hpp} (50%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/bicycle_corrector.hpp (74%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/bus_corrector.hpp (73%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/car_corrector.hpp (74%) create mode 100644 perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/corrector_interface.hpp (73%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/no_corrector.hpp (58%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/reference_shape_size_corrector.hpp (70%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/trailer_corrector.hpp (77%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/truck_corrector.hpp (74%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/utils.hpp (82%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/corrector/vehicle_corrector.hpp (75%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/bus_filter.hpp (70%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/car_filter.hpp (70%) create mode 100644 perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/filter_interface.hpp (75%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/no_filter.hpp (58%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/trailer_filter.hpp (70%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/truck_filter.hpp (70%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/filter/utils.hpp (77%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/model/bounding_box.hpp (80%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/model/convex_hull.hpp (71%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/model/cylinder.hpp (74%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/model/model.hpp (60%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/model/model_interface.hpp (78%) rename perception/shape_estimation/include/{ => autoware}/shape_estimation/shape_estimator.hpp (90%) delete mode 100644 perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp delete mode 100644 perception/shape_estimation/include/shape_estimation/filter/filter.hpp rename perception/shape_estimation/src/{node.cpp => shape_estimation_node.cpp} (93%) rename perception/shape_estimation/src/{node.hpp => shape_estimation_node.hpp} (88%) diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 112187551d924..c019b2be77587 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -17,21 +17,15 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) include_directories( - include SYSTEM ${EIGEN3_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -# Generate exe file -set(DETECTION_BY_TRACKER_SRC - src/detection_by_tracker_node.cpp - src/utils/utils.cpp -) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${DETECTION_BY_TRACKER_SRC} + src/detection_by_tracker_node.cpp + src/tracker/tracker_handler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -43,9 +37,7 @@ rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::detection_by_tracker::DetectionByTracker" EXECUTABLE detection_by_tracker_node ) - -ament_auto_package( - INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE launch config ) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 242056c3ca316..56f37fb10043d 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -20,7 +20,6 @@ #include "euclidean_cluster/euclidean_cluster.hpp" #include "euclidean_cluster/utils.hpp" #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" #include @@ -46,7 +45,6 @@ #include #include #include - namespace autoware::detection_by_tracker { class Debugger diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c41d6002d9266..c22d015aa11a4 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -56,24 +56,26 @@ autoware_perception_msgs::msg::Shape extendShape( return output; } -boost::optional getReferenceYawInfo(const uint8_t label, const float yaw) +boost::optional getReferenceYawInfo( + const uint8_t label, const float yaw) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; + return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; } else { return boost::none; } } -boost::optional getReferenceShapeSizeInfo( +boost::optional getReferenceShapeSizeInfo( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceShapeSizeInfo{shape, ReferenceShapeSizeInfo::Mode::Min}; + return autoware::shape_estimation::ReferenceShapeSizeInfo{ + shape, autoware::shape_estimation::ReferenceShapeSizeInfo::Mode::Min}; } else { return boost::none; } @@ -83,70 +85,6 @@ boost::optional getReferenceShapeSizeInfo( namespace autoware::detection_by_tracker { -void TrackerHandler::onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) -{ - constexpr size_t max_buffer_size = 10; - - // Add tracked objects to buffer - objects_buffer_.push_front(*msg); - - // Remove old data - while (max_buffer_size < objects_buffer_.size()) { - objects_buffer_.pop_back(); - } -} - -bool TrackerHandler::estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) -{ - if (objects_buffer_.empty()) { - return false; - } - - // Get the objects closest to the target time. - const auto target_objects_iter = std::min_element( - objects_buffer_.cbegin(), objects_buffer_.cend(), - [&time]( - autoware_perception_msgs::msg::TrackedObjects first, - autoware_perception_msgs::msg::TrackedObjects second) { - return std::fabs((time - first.header.stamp).seconds()) < - std::fabs((time - second.header.stamp).seconds()); - }); - - // Estimate the pose of the object at the target time - const auto dt = time - target_objects_iter->header.stamp; - output.header.frame_id = target_objects_iter->header.frame_id; - output.header.stamp = time; - for (const auto & object : target_objects_iter->objects) { - const auto & pose_with_covariance = object.kinematics.pose_with_covariance; - const auto & x = pose_with_covariance.pose.position.x; - const auto & y = pose_with_covariance.pose.position.y; - const auto & z = pose_with_covariance.pose.position.z; - const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); - const auto & twist = object.kinematics.twist_with_covariance.twist; - const float & vx = twist.linear.x; - const float & wz = twist.angular.z; - - // build output - autoware_perception_msgs::msg::TrackedObject estimated_object; - estimated_object.object_id = object.object_id; - estimated_object.existence_probability = object.existence_probability; - estimated_object.classification = object.classification; - estimated_object.shape = object.shape; - estimated_object.kinematics.pose_with_covariance.pose.position.x = - x + vx * std::cos(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.y = - y + vx * std::sin(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); - estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_hat); - output.objects.push_back(estimated_object); - } - return true; -} - DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("detection_by_tracker", node_options), tf_buffer_(this->get_clock()), @@ -176,7 +114,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) // set maximum search setting for merger/divider setMaxSearchRange(); - shape_estimator_ = std::make_shared(true, true); + shape_estimator_ = std::make_shared(true, true); cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); @@ -468,7 +406,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( out_objects.feature_objects.push_back(feature_object); } } - } // namespace autoware::detection_by_tracker #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 4e58d53fa71aa..095708fb4b51f 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,13 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" #include "euclidean_cluster/euclidean_cluster.hpp" #include "euclidean_cluster/utils.hpp" #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" #include @@ -52,19 +53,6 @@ namespace autoware::detection_by_tracker { -class TrackerHandler -{ -private: - std::deque objects_buffer_; - -public: - TrackerHandler() = default; - void onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); - bool estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); -}; - class DetectionByTracker : public rclcpp::Node { public: @@ -80,13 +68,13 @@ class DetectionByTracker : public rclcpp::Node tf2_ros::TransformListener tf_listener_; TrackerHandler tracker_handler_; - std::shared_ptr shape_estimator_; + std::shared_ptr shape_estimator_; std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; std::unique_ptr published_time_publisher_; @@ -112,7 +100,6 @@ class DetectionByTracker : public rclcpp::Node autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); }; - } // namespace autoware::detection_by_tracker #endif // DETECTION_BY_TRACKER_NODE_HPP_ diff --git a/perception/detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/detection_by_tracker/src/tracker/tracker_handler.cpp new file mode 100644 index 0000000000000..a099c8b1ba55d --- /dev/null +++ b/perception/detection_by_tracker/src/tracker/tracker_handler.cpp @@ -0,0 +1,88 @@ +// Copyright 2021 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 "tracker_handler.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" + +#include + +namespace autoware::detection_by_tracker +{ + +void TrackerHandler::onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) +{ + constexpr size_t max_buffer_size = 10; + + // Add tracked objects to buffer + objects_buffer_.push_front(*msg); + + // Remove old data + while (max_buffer_size < objects_buffer_.size()) { + objects_buffer_.pop_back(); + } +} + +bool TrackerHandler::estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) +{ + if (objects_buffer_.empty()) { + return false; + } + + // Get the objects closest to the target time. + const auto target_objects_iter = std::min_element( + objects_buffer_.cbegin(), objects_buffer_.cend(), + [&time]( + autoware_perception_msgs::msg::TrackedObjects first, + autoware_perception_msgs::msg::TrackedObjects second) { + return std::fabs((time - first.header.stamp).seconds()) < + std::fabs((time - second.header.stamp).seconds()); + }); + + // Estimate the pose of the object at the target time + const auto dt = time - target_objects_iter->header.stamp; + output.header.frame_id = target_objects_iter->header.frame_id; + output.header.stamp = time; + for (const auto & object : target_objects_iter->objects) { + const auto & pose_with_covariance = object.kinematics.pose_with_covariance; + const auto & x = pose_with_covariance.pose.position.x; + const auto & y = pose_with_covariance.pose.position.y; + const auto & z = pose_with_covariance.pose.position.z; + const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); + const auto & twist = object.kinematics.twist_with_covariance.twist; + const float & vx = twist.linear.x; + const float & wz = twist.angular.z; + + // build output + autoware_perception_msgs::msg::TrackedObject estimated_object; + estimated_object.object_id = object.object_id; + estimated_object.existence_probability = object.existence_probability; + estimated_object.classification = object.classification; + estimated_object.shape = object.shape; + estimated_object.kinematics.pose_with_covariance.pose.position.x = + x + vx * std::cos(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.y = + y + vx * std::sin(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.z = z; + const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); + estimated_object.kinematics.pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(yaw_hat); + output.objects.push_back(estimated_object); + } + return true; +} +} // namespace autoware::detection_by_tracker diff --git a/perception/detection_by_tracker/src/utils/utils.cpp b/perception/detection_by_tracker/src/tracker/tracker_handler.hpp similarity index 50% rename from perception/detection_by_tracker/src/utils/utils.cpp rename to perception/detection_by_tracker/src/tracker/tracker_handler.hpp index b269d703be7d4..13aa00aaf0c4c 100644 --- a/perception/detection_by_tracker/src/utils/utils.cpp +++ b/perception/detection_by_tracker/src/tracker/tracker_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// 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. @@ -12,22 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils.hpp" +#ifndef TRACKER__TRACKER_HANDLER_HPP_ +#define TRACKER__TRACKER_HANDLER_HPP_ -#include +#include + +#include "autoware_perception_msgs/msg/tracked_objects.hpp" + +#include namespace autoware::detection_by_tracker { -namespace utils -{ -using Label = autoware_perception_msgs::msg::ObjectClassification; -bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const +class TrackerHandler { - return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || - (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || - (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || - (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); -} -} // namespace utils +private: + std::deque objects_buffer_; + +public: + TrackerHandler() = default; + void onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); + bool estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); +}; + } // namespace autoware::detection_by_tracker + +#endif // TRACKER__TRACKER_HANDLER_HPP_ diff --git a/perception/detection_by_tracker/src/utils/utils.hpp b/perception/detection_by_tracker/src/utils/utils.hpp index b7041124fa931..cc29164e4aad9 100644 --- a/perception/detection_by_tracker/src/utils/utils.hpp +++ b/perception/detection_by_tracker/src/utils/utils.hpp @@ -15,12 +15,16 @@ #ifndef UTILS__UTILS_HPP_ #define UTILS__UTILS_HPP_ +#include "autoware_perception_msgs/msg/object_classification.hpp" + #include namespace autoware::detection_by_tracker { namespace utils { +using Label = autoware_perception_msgs::msg::ObjectClassification; + struct TrackerIgnoreLabel { bool UNKNOWN; @@ -31,8 +35,15 @@ struct TrackerIgnoreLabel bool MOTORCYCLE; bool BICYCLE; bool PEDESTRIAN; - bool isIgnore(const uint8_t label) const; -}; // struct TrackerIgnoreLabel + bool isIgnore(const uint8_t label) const + { + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); + } +}; + } // namespace utils } // namespace autoware::detection_by_tracker diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 527c565b91e05..21517f8c0f766 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -16,7 +16,7 @@ set(SHAPE_ESTIMATION_DEPENDENCIES Eigen3 ) -ament_auto_add_library(shape_estimation_lib SHARED +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/shape_estimator.cpp lib/model/bounding_box.cpp lib/model/convex_hull.cpp @@ -27,23 +27,23 @@ ament_auto_add_library(shape_estimation_lib SHARED lib/filter/trailer_filter.cpp lib/filter/no_filter.cpp lib/filter/utils.cpp - lib/corrector/no_corrector.cpp lib/corrector/utils.cpp + lib/corrector/no_corrector.cpp ) -ament_target_dependencies(shape_estimation_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME}_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_include_directories(shape_estimation_lib +target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC "${PCL_INCLUDE_DIRS}" "${EIGEN3_INCLUDE_DIR}" ) -ament_auto_add_library(shape_estimation_node SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/shape_estimation_node.cpp ) -target_include_directories(shape_estimation_node +target_include_directories(${PROJECT_NAME} PUBLIC $ $ @@ -51,15 +51,15 @@ target_include_directories(shape_estimation_node ${CMAKE_CURRENT_SOURCE_DIR}/src ) -ament_target_dependencies(shape_estimation_node ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME} ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_link_libraries(shape_estimation_node +target_link_libraries(${PROJECT_NAME} shape_estimation_lib ) -rclcpp_components_register_node(shape_estimation_node - PLUGIN "ShapeEstimationNode" - EXECUTABLE shape_estimation +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::shape_estimation::ShapeEstimationNode" + EXECUTABLE shape_estimation_node ) ament_auto_package(INSTALL_TO_SHARE @@ -79,6 +79,6 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) target_link_libraries(test_shape_estimation - shape_estimation_node + ${PROJECT_NAME} ) endif() diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index 206e8d422c727..9615b314f62c6 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,7 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} +{{ json_to_markdown("perception/autoware/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp index ec1a2b8a42973..f8dc489a563be 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class BicycleCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class BicycleCorrector : public VehicleCorrector ~BicycleCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp index 4ef1f00122ca4..d7386ace5b364 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace corrector +{ class BusCorrector : public VehicleCorrector { public: @@ -36,4 +39,7 @@ class BusCorrector : public VehicleCorrector ~BusCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp index d4d33599c7730..7292704d0f6d9 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class CarCorrector : public VehicleCorrector { public: @@ -36,4 +41,7 @@ class CarCorrector : public VehicleCorrector ~CarCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp new file mode 100644 index 0000000000000..dd4c4f68d3f46 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp @@ -0,0 +1,27 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// 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__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ + +#include "autoware/shape_estimation/corrector/bicycle_corrector.hpp" +#include "autoware/shape_estimation/corrector/bus_corrector.hpp" +#include "autoware/shape_estimation/corrector/car_corrector.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/no_corrector.hpp" +#include "autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp" +#include "autoware/shape_estimation/corrector/trailer_corrector.hpp" +#include "autoware/shape_estimation/corrector/truck_corrector.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp index 1ce1273f042b9..1dcf85b46887c 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ #include #include #include - +namespace autoware::shape_estimation +{ +namespace corrector +{ class ShapeEstimationCorrectorInterface { public: @@ -31,4 +34,7 @@ class ShapeEstimationCorrectorInterface autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp index 1303f6f859191..e3c33100861a0 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class NoCorrector : public ShapeEstimationCorrectorInterface { public: @@ -26,7 +31,14 @@ class NoCorrector : public ShapeEstimationCorrectorInterface ~NoCorrector() {} bool correct( - autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp index 0d1740dc30d61..a05027a3348b6 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInterface { ReferenceShapeSizeInfo ref_shape_size_info_; @@ -38,4 +43,6 @@ class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInter } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp index 6dd885353c78f..afd0f1f7c451d 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + // Generally speaking, trailer would be much larger than bus and truck. // But currently we do not make large differences among bus/truck/trailer // because current our vehicle classification is not reliable enough. @@ -40,4 +45,7 @@ class TrailerCorrector : public VehicleCorrector ~TrailerCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp index 3b2475286fa61..8a641e84176a5 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class TruckCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class TruckCorrector : public VehicleCorrector ~TruckCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp similarity index 82% rename from perception/shape_estimation/include/shape_estimation/corrector/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp index 951f3d16aa278..a16cb146ca4a3 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include +namespace autoware::shape_estimation +{ + namespace corrector_utils { struct CorrectionBBParameters @@ -46,4 +49,6 @@ bool correctWithReferenceYaw( } // namespace corrector_utils -#endif // SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp index 1b38f15620663..e1b292eb5318a 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class VehicleCorrector : public ShapeEstimationCorrectorInterface { protected: @@ -47,4 +52,7 @@ class VehicleCorrector : public ShapeEstimationCorrectorInterface void setParams(const corrector_utils::CorrectionBBParameters & params) { params_ = params; } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp index 301921b00d439..54d45498a3d40 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class BusFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class BusFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp index 79104823bb7ab..8ff79f901d8a3 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class CarFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class CarFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose) override; }; -#endif // SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp new file mode 100644 index 0000000000000..5b7bad4f57208 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp @@ -0,0 +1,25 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// 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__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ + +#include "autoware/shape_estimation/filter/bus_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/no_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp index 67dfe27eb93b0..cba7715314a9e 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ #include #include #include +namespace autoware::shape_estimation +{ +namespace filter +{ + class ShapeEstimationFilterInterface { public: @@ -31,4 +36,7 @@ class ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp index d007e6c61c965..f05c0e9affa02 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ class NoFilter : public ShapeEstimationFilterInterface { @@ -25,8 +30,14 @@ class NoFilter : public ShapeEstimationFilterInterface ~NoFilter() = default; bool filter( - const autoware_perception_msgs::msg::Shape & shape, - const geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] const geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp index 319d3e25c77c0..21394938a8026 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class TrailerFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +33,7 @@ class TrailerFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp index e67da1c31115e..a941ec4fd7932 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace filter +{ class TruckFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +32,7 @@ class TruckFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/filter/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp index 8d487b8d42a93..7f56664e0a57d 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__UTILS_HPP_ -#define SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ #include +namespace autoware::shape_estimation +{ namespace utils { @@ -24,4 +26,6 @@ bool filterVehicleBoundingBox( const float max_length); } // namespace utils -#endif // SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp similarity index 80% rename from perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp index 2ac179d030629..c9d6889c21de5 100644 --- a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#define SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#include "shape_estimation/model/model_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include +namespace autoware::shape_estimation +{ +namespace model +{ + class BoundingBoxShapeModel : public ShapeEstimationModelInterface { private: @@ -47,4 +52,7 @@ class BoundingBoxShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp similarity index 71% rename from perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp index 2748d4a53a807..c3ffa297833fb 100644 --- a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#define SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class ConvexHullShapeModel : public ShapeEstimationModelInterface { @@ -30,4 +35,7 @@ class ConvexHullShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/model/cylinder.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp index ccd28afa3173b..6623934cdcc21 100644 --- a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#define SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class CylinderShapeModel : public ShapeEstimationModelInterface { @@ -33,4 +38,7 @@ class CylinderShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp similarity index 60% rename from perception/shape_estimation/include/shape_estimation/model/model.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp index 65080ac884073..382812de6f188 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#include "shape_estimation/model/bounding_box.hpp" -#include "shape_estimation/model/convex_hull.hpp" -#include "shape_estimation/model/cylinder.hpp" -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/bounding_box.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" -#endif // SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp similarity index 78% rename from perception/shape_estimation/include/shape_estimation/model/model_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp index f5324196f69bd..380e88704baae 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #include #include @@ -24,6 +24,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + class ShapeEstimationModelInterface { public: @@ -37,4 +42,7 @@ class ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) = 0; }; -#endif // SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp similarity index 90% rename from perception/shape_estimation/include/shape_estimation/shape_estimator.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp index 7cdab6f86b585..9759c7c77855e 100644 --- a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ -#define SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + struct ReferenceYawInfo { float yaw; @@ -75,5 +78,6 @@ class ShapeEstimator const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output); }; +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp deleted file mode 100644 index e4efc17181e52..0000000000000 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// 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 SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ - -#include "shape_estimation/corrector/bicycle_corrector.hpp" -#include "shape_estimation/corrector/bus_corrector.hpp" -#include "shape_estimation/corrector/car_corrector.hpp" -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/corrector/no_corrector.hpp" -#include "shape_estimation/corrector/reference_shape_size_corrector.hpp" -#include "shape_estimation/corrector/trailer_corrector.hpp" -#include "shape_estimation/corrector/truck_corrector.hpp" - -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp deleted file mode 100644 index b205cbd6791ba..0000000000000 --- a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// 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 SHAPE_ESTIMATION__FILTER__FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_HPP_ - -#include "shape_estimation/filter/bus_filter.hpp" -#include "shape_estimation/filter/car_filter.hpp" -#include "shape_estimation/filter/filter_interface.hpp" -#include "shape_estimation/filter/no_filter.hpp" -#include "shape_estimation/filter/trailer_filter.hpp" -#include "shape_estimation/filter/truck_filter.hpp" - -#endif // SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index f13b5d1e5f273..88cea81412d97 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/shape_estimation/lib/corrector/no_corrector.cpp b/perception/shape_estimation/lib/corrector/no_corrector.cpp index e18de4acefcdd..b37ee289871dc 100644 --- a/perception/shape_estimation/lib/corrector/no_corrector.cpp +++ b/perception/shape_estimation/lib/corrector/no_corrector.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/no_corrector.hpp" - -bool NoCorrector::correct( - [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape_output, - [[maybe_unused]] geometry_msgs::msg::Pose & pose_output) -{ - return true; -} +#include "autoware/shape_estimation/corrector/no_corrector.hpp" diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index 1332647b0f1f4..a18e09d5b8a04 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/utils.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/corrector/utils.hpp" -#include +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -28,12 +30,14 @@ #include #endif +#include +#include + #include #include -#define EIGEN_MPL2_ONLY -#include -#include +namespace autoware::shape_estimation +{ namespace corrector_utils { @@ -398,3 +402,5 @@ bool correctWithReferenceYawAndShapeSize( return true; } } // namespace corrector_utils + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/bus_filter.cpp b/perception/shape_estimation/lib/filter/bus_filter.cpp index 536545813237f..a301ce716cca2 100644 --- a/perception/shape_estimation/lib/filter/bus_filter.cpp +++ b/perception/shape_estimation/lib/filter/bus_filter.cpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/bus_filter.hpp" - +#include "autoware/shape_estimation/filter/bus_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool BusFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +26,6 @@ bool BusFilter::filter( constexpr float max_length = 17.0; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/car_filter.cpp b/perception/shape_estimation/lib/filter/car_filter.cpp index e12b9c211a24e..ab7a8d69d109f 100644 --- a/perception/shape_estimation/lib/filter/car_filter.cpp +++ b/perception/shape_estimation/lib/filter/car_filter.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool CarFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +27,6 @@ bool CarFilter::filter( constexpr float max_length = 5.8; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/no_filter.cpp b/perception/shape_estimation/lib/filter/no_filter.cpp index 564de63461c4f..22690be2ed769 100644 --- a/perception/shape_estimation/lib/filter/no_filter.cpp +++ b/perception/shape_estimation/lib/filter/no_filter.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/no_filter.hpp" - -bool NoFilter::filter( - [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, - [[maybe_unused]] const geometry_msgs::msg::Pose & pose) -{ - return true; -} +#include "autoware/shape_estimation/filter/no_filter.hpp" diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/shape_estimation/lib/filter/trailer_filter.cpp index 5087350f4d60a..378ce085aa881 100644 --- a/perception/shape_estimation/lib/filter/trailer_filter.cpp +++ b/perception/shape_estimation/lib/filter/trailer_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TrailerFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TrailerFilter::filter( constexpr float max_length = 25.0; // maximum Full-TRAILER size in JAPAN(normally upto 18m) return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index ad733bb58c97c..4afcccaf55347 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/truck_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TruckFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TruckFilter::filter( constexpr float max_length = 18.0; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/utils.cpp b/perception/shape_estimation/lib/filter/utils.cpp index 9412be4e99a9e..616eb5e22769e 100644 --- a/perception/shape_estimation/lib/filter/utils.cpp +++ b/perception/shape_estimation/lib/filter/utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/utils.hpp" +#include "autoware/shape_estimation/filter/utils.hpp" +namespace autoware::shape_estimation +{ namespace utils { @@ -43,4 +45,6 @@ bool filterVehicleBoundingBox( } return true; } + } // namespace utils +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index a6693fa56d55b..df751be27d8c0 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/bounding_box.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/model/bounding_box.hpp" #include #include #include -#include +#include "autoware_perception_msgs/msg/shape.hpp" #include @@ -33,14 +35,17 @@ #include #endif +#include + #include #include #include #include -#define EIGEN_MPL2_ONLY - -#include +namespace autoware::shape_estimation +{ +namespace model +{ constexpr float epsilon = 0.001; @@ -252,3 +257,6 @@ float BoundingBoxShapeModel::boostOptimize( float theta_star = min.first; return theta_star; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/convex_hull.cpp b/perception/shape_estimation/lib/model/convex_hull.cpp index d29fb6aa90a99..3c68634aaff4c 100644 --- a/perception/shape_estimation/lib/model/convex_hull.cpp +++ b/perception/shape_estimation/lib/model/convex_hull.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" #include #include @@ -28,6 +28,11 @@ #include #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool ConvexHullShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -94,3 +99,6 @@ bool ConvexHullShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/cylinder.cpp b/perception/shape_estimation/lib/model/cylinder.cpp index 9b3f58854df9d..3f3139222a2bc 100644 --- a/perception/shape_estimation/lib/model/cylinder.cpp +++ b/perception/shape_estimation/lib/model/cylinder.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" #include #include @@ -26,6 +26,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool CylinderShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -63,3 +68,6 @@ bool CylinderShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 85bc3cd8bd9d9..34eed5e641368 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer) @@ -75,15 +78,15 @@ bool ShapeEstimator::estimateOriginalShapeAndPose( autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) { // estimate shape - std::unique_ptr model_ptr; + std::unique_ptr model_ptr; if ( label == Label::CAR || label == Label::TRUCK || label == Label::BUS || label == Label::TRAILER || label == Label::MOTORCYCLE || label == Label::BICYCLE) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); + model_ptr.reset(new model::BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); } else if (label == Label::PEDESTRIAN) { - model_ptr.reset(new CylinderShapeModel()); + model_ptr.reset(new model::CylinderShapeModel()); } else { - model_ptr.reset(new ConvexHullShapeModel()); + model_ptr.reset(new model::ConvexHullShapeModel()); } return model_ptr->estimate(cluster, shape_output, pose_output); @@ -93,17 +96,17 @@ bool ShapeEstimator::applyFilter( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) { - std::unique_ptr filter_ptr; + std::unique_ptr filter_ptr; if (label == Label::CAR) { - filter_ptr.reset(new CarFilter); + filter_ptr.reset(new filter::CarFilter); } else if (label == Label::BUS) { - filter_ptr.reset(new BusFilter); + filter_ptr.reset(new filter::BusFilter); } else if (label == Label::TRUCK) { - filter_ptr.reset(new TruckFilter); + filter_ptr.reset(new filter::TruckFilter); } else if (label == Label::TRAILER) { - filter_ptr.reset(new TrailerFilter); + filter_ptr.reset(new filter::TrailerFilter); } else { - filter_ptr.reset(new NoFilter); + filter_ptr.reset(new filter::NoFilter); } return filter_ptr->filter(shape, pose); @@ -114,23 +117,26 @@ bool ShapeEstimator::applyCorrector( const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) { - std::unique_ptr corrector_ptr; + std::unique_ptr corrector_ptr; if (ref_shape_size_info && use_reference_yaw) { - corrector_ptr.reset(new ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); + corrector_ptr.reset( + new corrector::ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); } else if (label == Label::CAR) { - corrector_ptr.reset(new CarCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::CarCorrector(use_reference_yaw)); } else if (label == Label::BUS) { - corrector_ptr.reset(new BusCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BusCorrector(use_reference_yaw)); } else if (label == Label::TRUCK) { - corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { - corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TrailerCorrector(use_reference_yaw)); } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { - corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BicycleCorrector(use_reference_yaw)); } else { - corrector_ptr.reset(new NoCorrector); + corrector_ptr.reset(new corrector::NoCorrector); } return corrector_ptr->correct(shape, pose); } + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/shape_estimation_node.cpp similarity index 93% rename from perception/shape_estimation/src/node.cpp rename to perception/shape_estimation/src/shape_estimation_node.cpp index 5719cd97b3ed5..4a373b15d580f 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/shape_estimation_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "shape_estimation_node.hpp" -#include -#include +#include "autoware/shape_estimation/shape_estimator.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include @@ -32,6 +32,9 @@ #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_options) @@ -143,6 +146,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } +} // namespace autoware::shape_estimation #include -RCLCPP_COMPONENTS_REGISTER_NODE(ShapeEstimationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::shape_estimation::ShapeEstimationNode) diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/shape_estimation_node.hpp similarity index 88% rename from perception/shape_estimation/src/node.hpp rename to perception/shape_estimation/src/shape_estimation_node.hpp index 807f12e39b025..d9f944346611a 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/shape_estimation_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef SHAPE_ESTIMATION_NODE_HPP_ +#define SHAPE_ESTIMATION_NODE_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + using autoware_perception_msgs::msg::DetectedObjects; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; class ShapeEstimationNode : public rclcpp::Node @@ -51,5 +54,6 @@ class ShapeEstimationNode : public rclcpp::Node public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); }; +} // namespace autoware::shape_estimation -#endif // NODE_HPP_ +#endif // SHAPE_ESTIMATION_NODE_HPP_ diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp index 0c41e04e5ddd2..2e6dcc62d8699 100644 --- a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp +++ b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -70,7 +70,7 @@ pcl::PointCloud createLShapeCluster( TEST(BoundingBoxShapeModel, test_estimateShape) { // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel(); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel(); // Generate cluster const double length = 4.0; @@ -117,12 +117,12 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) pcl::PointCloud cluster = createLShapeCluster(length, width, height, yaw, offset_x, offset_y); - const auto ref_yaw_info = - ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const auto ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const bool use_boost_bbox_optimizer = true; // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = - BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel( + ref_yaw_info, use_boost_bbox_optimizer); // Generate shape and pose output autoware_perception_msgs::msg::Shape shape_output; @@ -152,7 +152,7 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) TEST(CylinderShapeModel, test_estimateShape) { // Generate CylinderShapeModel - CylinderShapeModel cylinder_shape_model = CylinderShapeModel(); + auto cylinder_shape_model = autoware::shape_estimation::model::CylinderShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0); @@ -169,7 +169,7 @@ TEST(CylinderShapeModel, test_estimateShape) TEST(ConvexHullShapeModel, test_estimateShape) { // Generate ConvexHullShapeModel - ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel(); + auto convex_hull_shape_model = autoware::shape_estimation::model::ConvexHullShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0); @@ -200,14 +200,16 @@ TEST(ShapeEstimator, test_estimateShapeAndPose) const bool use_corrector = true; const bool use_filter = true; const bool use_boost_bbox_optimizer = true; - ShapeEstimator shape_estimator = - ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); + auto shape_estimator = + autoware::shape_estimation::ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); // Generate ref_yaw_info - boost::optional ref_yaw_info = boost::none; - boost::optional ref_shape_size_info = boost::none; + boost::optional ref_yaw_info = boost::none; + boost::optional ref_shape_size_info = + boost::none; - ref_yaw_info = ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const auto label = autoware_perception_msgs::msg::ObjectClassification::CAR; // Generate shape and pose output From 07e226c5c48fa5223f1b4b75316dfbbeece6a733 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 08:10:59 +0900 Subject: [PATCH 17/33] fix(pointcloud_preprocessor): temporarily remove distortion corrector unit test (#7879) fix: temporarily remove distortion corretor unit test Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 4 - .../test/test_distortion_corrector_node.cpp | 600 ------------------ 2 files changed, 604 deletions(-) delete mode 100644 sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 2d0649fe43954..53b6aae55d47b 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -231,12 +231,8 @@ if(BUILD_TESTING) ament_add_gtest(test_utilities test/test_utilities.cpp ) - ament_add_gtest(distortion_corrector_node_tests - test/test_distortion_corrector_node.cpp - ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) - target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp deleted file mode 100644 index 9e5d068be52f5..0000000000000 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ /dev/null @@ -1,600 +0,0 @@ -// 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 "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" - -#include - -#include -#include -#include -#include - -#include -#include - -class DistortionCorrectorTest : public ::testing::Test -{ -protected: - void SetUp() override - { - node_ = std::make_shared("test_node"); - distortion_corrector_2d_ = - std::make_shared(node_.get()); - distortion_corrector_3d_ = - std::make_shared(node_.get()); - - // Setup TF - tf_broadcaster_ = std::make_shared(node_); - tf_broadcaster_->sendTransform(generateStaticTransformMsg()); - - // Spin the node for a while to ensure transforms are published - auto start = std::chrono::steady_clock::now(); - auto timeout = std::chrono::seconds(1); - while (std::chrono::steady_clock::now() - start < timeout) { - rclcpp::spin_some(node_); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - - void TearDown() override {} - - rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) - { - auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); - return stamp + ms_in_ns; - } - - rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) - { - auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); - return stamp - ms_in_ns; - } - - geometry_msgs::msg::TransformStamped generateTransformMsg( - const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, - double qx, double qy, double qz, double qw) - { - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = node_->get_clock()->now(); - tf_msg.header.frame_id = parent_frame; - tf_msg.child_frame_id = child_frame; - tf_msg.transform.translation.x = x; - tf_msg.transform.translation.y = y; - tf_msg.transform.translation.z = z; - tf_msg.transform.rotation.x = qx; - tf_msg.transform.rotation.y = qy; - tf_msg.transform.rotation.z = qz; - tf_msg.transform.rotation.w = qw; - return tf_msg; - } - - std::vector generateStaticTransformMsg() - { - return { - generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), - generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; - } - - std::shared_ptr generateTwistMsg( - double linear_x, double angular_z, rclcpp::Time stamp) - { - auto twist_msg = std::make_shared(); - twist_msg->header.stamp = stamp; - twist_msg->header.frame_id = "base_link"; - twist_msg->twist.twist.linear.x = linear_x; - twist_msg->twist.twist.angular.z = angular_z; - return twist_msg; - } - - std::shared_ptr generateImuMsg( - double angular_vel_x, double angular_vel_y, double angular_vel_z, rclcpp::Time stamp) - { - auto imu_msg = std::make_shared(); - imu_msg->header.stamp = stamp; - imu_msg->header.frame_id = "imu_link"; - imu_msg->angular_velocity.x = angular_vel_x; - imu_msg->angular_velocity.y = angular_vel_y; - imu_msg->angular_velocity.z = angular_vel_z; - return imu_msg; - } - - std::vector> generateTwistMsgs( - rclcpp::Time pointcloud_timestamp) - { - std::vector> twist_msgs; - rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); - - for (int i = 0; i < 6; ++i) { - auto twist_msg = generateTwistMsg(10.0 + i * 2, 0.02 + i * 0.01, twist_stamp); - twist_msgs.push_back(twist_msg); - // Make sure the twist stamp is not identical to any point stamp, and imu stamp - twist_stamp = addMilliseconds(twist_stamp, 24); - } - - return twist_msgs; - } - - std::vector> generateImuMsgs( - rclcpp::Time pointcloud_timestamp) - { - std::vector> imu_msgs; - rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); - - for (int i = 0; i < 6; ++i) { - auto imu_msg = - generateImuMsg(0.01 + i * 0.005, -0.02 + i * 0.005, 0.05 + i * 0.005, imu_stamp); - imu_msgs.push_back(imu_msg); - // Make sure the imu stamp is not identical to any point stamp, and twist stamp - imu_stamp = addMilliseconds(imu_stamp, 27); - } - - return imu_msgs; - } - - sensor_msgs::msg::PointCloud2 generatePointCloudMsg( - bool is_generate_points, bool is_lidar_frame, rclcpp::Time stamp) - { - sensor_msgs::msg::PointCloud2 pointcloud_msg; - pointcloud_msg.header.stamp = stamp; - pointcloud_msg.header.frame_id = is_lidar_frame ? "lidar_top" : "base_link"; - pointcloud_msg.height = 1; - pointcloud_msg.is_dense = true; - pointcloud_msg.is_bigendian = false; - pointcloud_msg.point_step = - 20; // 3 float32 fields * 4 bytes/field + 1 float64 field * 8 bytes/field - - if (is_generate_points) { - std::vector points = { - 10.0f, 0.0f, 0.0f, // point 1 - 0.0f, 10.0f, 0.0f, // point 2 - 0.0f, 0.0f, 10.0f, // point 3 - 20.0f, 0.0f, 0.0f, // point 4 - 0.0f, 20.0f, 0.0f, // point 5 - 0.0f, 0.0f, 20.0f, // point 6 - 30.0f, 0.0f, 0.0f, // point 7 - 0.0f, 30.0f, 0.0f, // point 8 - 0.0f, 0.0f, 30.0f, // point 9 - 10.0f, 10.0f, 10.0f // point 10 - }; - - size_t number_of_points = points.size() / 3; - std::vector timestamps = - generatePointTimestamps(is_generate_points, stamp, number_of_points); - - std::vector data(number_of_points * pointcloud_msg.point_step); - - for (size_t i = 0; i < number_of_points; ++i) { - std::memcpy(data.data() + i * pointcloud_msg.point_step, &points[i * 3], 3 * sizeof(float)); - std::memcpy( - data.data() + i * pointcloud_msg.point_step + 12, ×tamps[i], sizeof(double)); - } - - pointcloud_msg.width = number_of_points; - pointcloud_msg.row_step = pointcloud_msg.point_step * pointcloud_msg.width; - pointcloud_msg.data = std::move(data); - } else { - pointcloud_msg.width = 0; - pointcloud_msg.row_step = 0; - } - - sensor_msgs::msg::PointField x_field; - x_field.name = "x"; - x_field.offset = 0; - x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - x_field.count = 1; - - sensor_msgs::msg::PointField y_field; - y_field.name = "y"; - y_field.offset = 4; - y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - y_field.count = 1; - - sensor_msgs::msg::PointField z_field; - z_field.name = "z"; - z_field.offset = 8; - z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - z_field.count = 1; - - sensor_msgs::msg::PointField timestamp_field; - timestamp_field.name = "time_stamp"; - timestamp_field.offset = 12; - timestamp_field.datatype = sensor_msgs::msg::PointField::FLOAT64; - timestamp_field.count = 1; - - pointcloud_msg.fields = {x_field, y_field, z_field, timestamp_field}; - - return pointcloud_msg; - } - - std::vector generatePointTimestamps( - bool is_generate_points, rclcpp::Time pointcloud_timestamp, size_t number_of_points) - { - std::vector timestamps; - if (is_generate_points) { - rclcpp::Time point_stamp = pointcloud_timestamp; - for (size_t i = 0; i < number_of_points; ++i) { - double timestamp = point_stamp.seconds(); - timestamps.push_back(timestamp); - if (i > 0) { - point_stamp = point_stamp + rclcpp::Duration::from_seconds( - 0.001); // Assuming 1ms offset for demonstration - } - } - } - return timestamps; - } - - std::shared_ptr node_; - std::shared_ptr distortion_corrector_2d_; - std::shared_ptr distortion_corrector_3d_; - std::shared_ptr tf_broadcaster_; -}; - -TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) -{ - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); - distortion_corrector_2d_->processTwistMessage(twist_msg); - - ASSERT_FALSE(distortion_corrector_2d_->twist_queue_.empty()); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.linear.x, 1.0); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.angular.z, 0.5); -} - -TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) -{ - auto imu_msg = generateImuMsg(0.5, 0.3, 0.1, node_->get_clock()->now()); - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - - ASSERT_FALSE(distortion_corrector_2d_->angular_velocity_queue_.empty()); - EXPECT_NEAR(distortion_corrector_2d_->angular_velocity_queue_.front().vector.z, 0.0443032, 1e-5); -} - -TEST_F(DistortionCorrectorTest, TestIsInputValid) -{ - // input normal pointcloud without twist - sensor_msgs::msg::PointCloud2 pointcloud = - generatePointCloudMsg(true, false, node_->get_clock()->now()); - bool result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_FALSE(result); - - // input normal pointcloud with valid twist - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); - distortion_corrector_2d_->processTwistMessage(twist_msg); - - pointcloud = generatePointCloudMsg(true, false, node_->get_clock()->now()); - result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_TRUE(result); - - // input empty pointcloud - pointcloud = generatePointCloudMsg(false, false, node_->get_clock()->now()); - result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_FALSE(result); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) -{ - rclcpp::Time timestamp = node_->get_clock()->now(); - // Generate the point cloud message - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Process empty twist queue - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); - - // Verify the point cloud is not changed - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0f, 0.0f, 10.0f}, {20.0f, 0.0f, 0.0f}, - {0.0f, 20.0f, 0.0f}, {0.0f, 0.0f, 20.0f}, {30.0f, 0.0f, 0.0f}, {0.0f, 30.0f, 0.0f}, - {0.0f, 0.0f, 30.0f}, {10.0f, 10.0f, 10.0f}, - }; - - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 1e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) -{ - rclcpp::Time timestamp = node_->get_clock()->now(); - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - // Generate an empty point cloud message - sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp); - - // Process empty point cloud - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud); - - // Verify the point cloud is still empty - EXPECT_EQ(empty_pointcloud.width, 0); - EXPECT_EQ(empty_pointcloud.row_step, 0); -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Test using only twist - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.012002f, 5.75338e-07f, 10.0f}, - {20.024f, 0.00191863f, 0.0f}, {0.0340828f, 20.0f, 0.0f}, {0.0479994f, 4.02654e-06f, 20.0f}, - {30.06f, 0.00575818f, 0.0f}, {0.0662481f, 30.0f, 0.0f}, {0.0839996f, 1.03542e-05f, 30.0f}, - {10.0931f, 10.0029f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 5.75201e-07f, 10.0f}, - {20.024f, 0.0019192f, 0.0f}, {0.0321653f, 20.0f, 0.0f}, {0.0479994f, 6.32762e-06f, 20.0f}, - {30.06f, 0.00863842f, 0.0f}, {0.063369f, 30.0f, 0.0f}, {0.0839996f, 1.84079e-05f, 30.0f}, - {10.0912f, 10.0048f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, -1.77636e-15f, -4.44089e-16f}, {-2.66454e-15f, 10.0f, -8.88178e-16f}, - {0.00622853f, 0.00600991f, 10.0084f}, {20.0106f, 0.010948f, 0.0157355f}, - {0.0176543f, 20.0176f, 0.0248379f}, {0.024499f, 0.0245179f, 20.0348f}, - {30.0266f, 0.0255826f, 0.0357166f}, {0.0355204f, 30.0353f, 0.0500275f}, - {0.047132f, 0.0439795f, 30.0606f}, {10.0488f, 10.046f, 10.0636f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Test using only twist - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(false, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 0.0f, 10.0f}, - {20.024f, 0.00120042f, 0.0f}, {0.0342002f, 20.0f, 0.0f}, {0.0479994f, 2.15994e-06f, 20.0f}, - {30.06f, 0.00450349f, 0.0f}, {0.0666005f, 30.0f, 0.0f}, {0.0839996f, 7.55993e-06f, 30.0f}, - {10.0936f, 10.0024f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {0.0f, 10.0f, 0.0f}, - {0.0118491f, -0.000149993f, 10.0f}, - {20.024f, 0.00220075f, 0.000600241f}, - {0.0327002f, 20.0f, 0.000900472f}, - {0.0468023f, -0.00119623f, 20.0f}, - {30.06f, 0.0082567f, 0.00225216f}, - {0.0621003f, 30.0f, 0.00270227f}, - {0.0808503f, -0.00313673f, 30.0f}, - {10.0904f, 10.0032f, 10.0024f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {-4.76837e-07f, 10.0f, 4.76837e-07f}, - {0.00572586f, 0.00616837f, 10.0086f}, - {20.0103f, 0.0117249f, 0.0149349f}, - {0.0158343f, 20.0179f, 0.024497f}, - {0.0251098f, 0.0254798f, 20.0343f}, - {30.0259f, 0.0290527f, 0.034577f}, - {0.0319824f, 30.0358f, 0.0477753f}, - {0.0478067f, 0.0460052f, 30.06f}, - {10.0462f, 10.0489f, 10.0625f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} From e1cd1da4dee812d508a3ec6ed615855721563e23 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 8 Jul 2024 09:10:41 +0900 Subject: [PATCH 18/33] refactor(gnss_poser): apply static analysis (#7874) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/gnss_poser/gnss_poser_core.hpp | 26 +++---- sensing/gnss_poser/src/gnss_poser_core.cpp | 71 ++++++++++--------- 2 files changed, 50 insertions(+), 47 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 97ca1d8bffe77..033abf04f1255 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -49,29 +49,29 @@ class GNSSPoser : public rclcpp::Node private: using MapProjectorInfo = map_interface::MapProjectorInfo; - void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); - void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); - void callbackGnssInsOrientationStamped( + void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); + void callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); - bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); - bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - geometry_msgs::msg::Point getMedianPosition( + static bool is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); + static bool can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); + static geometry_msgs::msg::Point get_median_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Point getAveragePosition( + static geometry_msgs::msg::Point get_average_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); - geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( + static geometry_msgs::msg::Quaternion get_quaternion_by_heading(const int heading); + static geometry_msgs::msg::Quaternion get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); - bool getTransform( + bool get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); - bool getStaticTransform( + bool get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp); - void publishTF( + void publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg); @@ -99,7 +99,7 @@ class GNSSPoser : public rclcpp::Node autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int gnss_pose_pub_method; + int gnss_pose_pub_method_; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7a3b40336ff3d..40a60d17ea7db 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -36,25 +36,27 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) + gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_sub( - sub_map_projector_info_, - [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); + sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { + callback_map_projector_info(msg); + }); // Set up position buffer - int buff_epoch = declare_parameter("buff_epoch"); + int buff_epoch = static_cast(declare_parameter("buff_epoch")); position_buffer_.set_capacity(buff_epoch); // Set subscribers and publishers nav_sat_fix_sub_ = create_subscription( - "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1)); + "fix", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callback_nav_sat_fix, this, std::placeholders::_1)); autoware_orientation_sub_ = create_subscription( "autoware_orientation", rclcpp::QoS{1}, - std::bind(&GNSSPoser::callbackGnssInsOrientationStamped, this, std::placeholders::_1)); + std::bind(&GNSSPoser::callback_gnss_ins_orientation_stamped, this, std::placeholders::_1)); pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); pose_cov_pub_ = create_publisher( @@ -68,13 +70,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; } -void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; received_map_projector_info_ = true; } -void GNSSPoser::callbackNavSatFix( +void GNSSPoser::callback_nav_sat_fix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { // Return immediately if map_projector_info has not been received yet. @@ -94,15 +96,15 @@ void GNSSPoser::callbackNavSatFix( } // check fixed topic - const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); + const bool is_status_fixed = is_fixed(nav_sat_fix_msg_ptr->status); // publish is_fixed topic tier4_debug_msgs::msg::BoolStamped is_fixed_msg; is_fixed_msg.stamp = this->now(); - is_fixed_msg.data = is_fixed; + is_fixed_msg.data = is_status_fixed; fixed_pub_->publish(is_fixed_msg); - if (!is_fixed) { + if (!is_status_fixed) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "Not Fixed Topic. Skipping Calculate."); @@ -122,7 +124,7 @@ void GNSSPoser::callbackNavSatFix( geometry_msgs::msg::Pose gnss_antenna_pose{}; // publish pose immediately - if (!gnss_pose_pub_method) { + if (!gnss_pose_pub_method_) { gnss_antenna_pose.position = position; } else { // fill position buffer @@ -134,8 +136,9 @@ void GNSSPoser::callbackNavSatFix( return; } // publish average pose or median pose of position buffer - gnss_antenna_pose.position = (gnss_pose_pub_method == 1) ? getAveragePosition(position_buffer_) - : getMedianPosition(position_buffer_); + gnss_antenna_pose.position = (gnss_pose_pub_method_ == 1) + ? get_average_position(position_buffer_) + : get_median_position(position_buffer_); } // calc gnss antenna orientation @@ -144,7 +147,7 @@ void GNSSPoser::callbackNavSatFix( orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; } else { static auto prev_position = gnss_antenna_pose.position; - orientation = getQuaternionByPositionDifference(gnss_antenna_pose.position, prev_position); + orientation = get_quaternion_by_position_difference(gnss_antenna_pose.position, prev_position); prev_position = gnss_antenna_pose.position; } @@ -157,7 +160,7 @@ void GNSSPoser::callbackNavSatFix( auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; - getStaticTransform( + get_static_transform( gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); @@ -179,11 +182,11 @@ void GNSSPoser::callbackNavSatFix( gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; gnss_base_pose_cov_msg.pose.covariance[7 * 0] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 1] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 2] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; if (use_gnss_ins_orientation_) { gnss_base_pose_cov_msg.pose.covariance[7 * 3] = @@ -201,30 +204,30 @@ void GNSSPoser::callbackNavSatFix( pose_cov_pub_->publish(gnss_base_pose_cov_msg); // broadcast map to gnss_base_link - publishTF(map_frame_, gnss_base_frame_, gnss_base_pose_msg); + publish_tf(map_frame_, gnss_base_frame_, gnss_base_pose_msg); } -void GNSSPoser::callbackGnssInsOrientationStamped( +void GNSSPoser::callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) { *msg_gnss_ins_orientation_stamped_ = *msg; } -bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) +bool GNSSPoser::is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) { return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; } -bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) +bool GNSSPoser::can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) { return nav_sat_fix_msg.position_covariance_type > sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -geometry_msgs::msg::Point GNSSPoser::getMedianPosition( +geometry_msgs::msg::Point GNSSPoser::get_median_position( const boost::circular_buffer & position_buffer) { - auto getMedian = [](std::vector array) { + auto get_median = [](std::vector array) { std::sort(std::begin(array), std::end(array)); const size_t median_index = array.size() / 2; double median = (array.size() % 2) @@ -243,13 +246,13 @@ geometry_msgs::msg::Point GNSSPoser::getMedianPosition( } geometry_msgs::msg::Point median_point; - median_point.x = getMedian(array_x); - median_point.y = getMedian(array_y); - median_point.z = getMedian(array_z); + median_point.x = get_median(array_x); + median_point.y = get_median(array_y); + median_point.z = get_median(array_z); return median_point; } -geometry_msgs::msg::Point GNSSPoser::getAveragePosition( +geometry_msgs::msg::Point GNSSPoser::get_average_position( const boost::circular_buffer & position_buffer) { std::vector array_x; @@ -271,7 +274,7 @@ geometry_msgs::msg::Point GNSSPoser::getAveragePosition( return average_point; } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int heading) +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_heading(const int heading) { int heading_conv = 0; // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] @@ -288,7 +291,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int headi return tf2::toMsg(quaternion); } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) { const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); @@ -297,7 +300,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( return tf2::toMsg(quaternion); } -bool GNSSPoser::getTransform( +bool GNSSPoser::get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) { @@ -340,7 +343,7 @@ bool GNSSPoser::getTransform( return true; } -bool GNSSPoser::getStaticTransform( +bool GNSSPoser::get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp) @@ -385,7 +388,7 @@ bool GNSSPoser::getStaticTransform( return true; } -void GNSSPoser::publishTF( +void GNSSPoser::publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) { From 4dc308000b44ece5ca0df03454a97ef86bc4b9e1 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 11:18:31 +0900 Subject: [PATCH 19/33] fix(pointcloud_preprocessor): fix documentation (#7878) fix: fix wrong name for json file Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 96124f473f825..033e82607057a 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,7 +36,7 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }} +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} ## Launch From fa6d8c583539cfd30d3eb537e4bcdd5fc5ef03c4 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 13:47:05 +0900 Subject: [PATCH 20/33] chore: update CODEOWNERS (#7819) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions Co-authored-by: Kenzo Lobos Tsunekawa --- .github/CODEOWNERS | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 8f947ce2d2a13..0fe324f84f81a 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,7 +6,7 @@ common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.j common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -115,9 +115,9 @@ perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4 perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp +perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -192,10 +192,10 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_modu planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp @@ -204,7 +204,7 @@ sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minod sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp +sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp From 598d9375e915758711c7b63e0e9446a81efc0430 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 8 Jul 2024 14:49:05 +0900 Subject: [PATCH 21/33] fix(motion_planning): fix processing time topic names (#7885) Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 4 ++-- .../autoware_motion_velocity_planner_node/src/node.hpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index f0100f58074ba..f181b0c76b51e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -83,8 +83,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); - processing_time_publisher_ = this->create_publisher( - "~/debug/total_time/processing_time_ms", 1); + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 8debb9cbedf00..4bf24cadb36f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -97,7 +97,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; From 45a0c3abd0a7436e650b937c5b8da8833f0ba535 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 15:53:24 +0900 Subject: [PATCH 22/33] refactor(probabilistic_occupancy_grid_map)!: fix namespace and directory structure (#7872) * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update namespace for nodes Signed-off-by: Taekjin LEE * refactor: update namespace for lib Signed-off-by: Taekjin LEE * refactor: remove unused dependency Signed-off-by: Taekjin LEE * refactor: use const pointer for occupancy grid map data Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 35 ++++++++++--------- .../cost_value}/cost_value.hpp | 14 +++++--- .../costmap_2d}/occupancy_grid_map.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_base.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_fixed.hpp | 11 +++--- .../occupancy_grid_map_projective.hpp | 11 +++--- .../fusion_policy/fusion_policy.hpp} | 12 ++++--- .../updater/binary_bayes_filter_updater.hpp} | 11 +++--- .../log_odds_bayes_filter_updater.hpp} | 15 ++++---- .../updater/ogm_updater_interface.hpp} | 14 ++++---- .../utils/utils.hpp | 12 ++++--- ...serscan_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- .../costmap_2d}/occupancy_grid_map.cpp | 17 +++++---- .../costmap_2d}/occupancy_grid_map_base.cpp | 12 ++++--- .../costmap_2d}/occupancy_grid_map_fixed.cpp | 29 ++++++++------- .../occupancy_grid_map_projective.cpp | 26 ++++++++------ .../fusion_policy/fusion_policy.cpp} | 9 +++-- .../updater/binary_bayes_filter_updater.cpp} | 13 ++++--- .../log_odds_bayes_filter_updater.cpp} | 10 ++++-- .../{src => lib}/utils/utils.cpp | 15 ++++---- .../package.xml | 5 +-- .../synchronized_grid_map_fusion_node.cpp | 20 +++++------ .../synchronized_grid_map_fusion_node.hpp | 18 +++++----- ...aserscan_based_occupancy_grid_map_node.cpp | 14 ++++---- ...aserscan_based_occupancy_grid_map_node.hpp | 16 ++++----- ...intcloud_based_occupancy_grid_map_node.cpp | 18 +++++----- ...intcloud_based_occupancy_grid_map_node.hpp | 16 ++++----- .../test/cost_value_test.cpp | 6 ++-- .../test/fusion_policy_test.cpp | 9 ++--- .../test/test_utils.cpp | 12 +++---- 32 files changed, 241 insertions(+), 183 deletions(-) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/cost_value}/cost_value.hpp (90%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_base.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_fixed.hpp (74%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_projective.hpp (77%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp => autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp} (84%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp} (72%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp} (69%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp => autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp} (68%) rename perception/probabilistic_occupancy_grid_map/include/{ => autoware}/probabilistic_occupancy_grid_map/utils/utils.hpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/laserscan_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_base.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_fixed.cpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_projective.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/fusion/single_frame_fusion_policy.cpp => lib/fusion_policy/fusion_policy.cpp} (97%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp => lib/updater/binary_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp => lib/updater/log_odds_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src => lib}/utils/utils.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/fusion/synchronized_grid_map_fusion_node.hpp (85%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp (82%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp (81%) diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 928532f928e38..d01ef157e21b4 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -14,8 +14,8 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME}_common SHARED - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/updater/binary_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} @@ -24,9 +24,9 @@ target_link_libraries(${PROJECT_NAME}_common # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp + lib/costmap_2d/occupancy_grid_map_base.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/costmap_2d/occupancy_grid_map_projective.cpp ) target_link_libraries(pointcloud_based_occupancy_grid_map @@ -35,14 +35,14 @@ target_link_libraries(pointcloud_based_occupancy_grid_map ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" EXECUTABLE pointcloud_based_occupancy_grid_map_node ) # LaserscanBasedOccupancyGridMap ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp - src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp + lib/costmap_2d/occupancy_grid_map.cpp ) target_link_libraries(laserscan_based_occupancy_grid_map @@ -51,17 +51,17 @@ target_link_libraries(laserscan_based_occupancy_grid_map ) rclcpp_components_register_node(laserscan_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" EXECUTABLE laserscan_based_occupancy_grid_map_node ) # GridMapFusionNode ament_auto_add_library(synchronized_grid_map_fusion SHARED src/fusion/synchronized_grid_map_fusion_node.cpp - src/fusion/single_frame_fusion_policy.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/fusion_policy/fusion_policy.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/updater/log_odds_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(synchronized_grid_map_fusion @@ -69,7 +69,7 @@ target_link_libraries(synchronized_grid_map_fusion ) rclcpp_components_register_node(synchronized_grid_map_fusion - PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode" EXECUTABLE synchronized_grid_map_fusion_node ) @@ -88,13 +88,14 @@ if(BUILD_TESTING) # gtest ament_add_gtest(test_utils - test/test_utils.cpp + test/test_utils.cpp ) ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp) + test/cost_value_test.cpp + ) ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - src/fusion/single_frame_fusion_policy.cpp + test/fusion_policy_test.cpp + lib/fusion_policy/fusion_policy.cpp ) target_link_libraries(test_utils ${PCL_LIBRARIES} diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp index 880297a1210ed..d470d206c41c6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp @@ -48,12 +48,14 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ #include -namespace occupancy_cost_value +namespace autoware::occupancy_grid_map +{ +namespace cost_value { static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; @@ -101,6 +103,8 @@ struct InverseCostTranslationTable static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; -} // namespace occupancy_cost_value -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +} // namespace cost_value +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index d2210cf9c348a..93e768c0f6b4e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -90,5 +92,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index f01b2d74e160b..2b2057af9cc12 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -91,5 +93,6 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 74% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 298f327d632d7..9dfd8b8689123 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -43,5 +45,6 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 77% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 67b51d6184c8c..1f0f92d933ff3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -51,5 +53,6 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp index 1f0553878ef5a..a2d7c90a023a5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -25,6 +25,8 @@ /*min and max prob threshold to prevent log-odds to diverge*/ #define EPSILON_PROB 0.03 +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion( unsigned char singleFrameOccupancyFusion( const std::vector & occupancy, FusionMethod method, const std::vector & reliability); + } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index 3a921035ef219..af28b7962b3bc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface @@ -38,5 +40,6 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 69% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index 245484e442609..d7bb1184c06d6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -25,6 +25,8 @@ // LOBF means: Log Odds Bayes Filter // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface @@ -45,5 +47,6 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 68% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index e85edf2245ef3..75231089ac33c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D @@ -27,8 +29,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D public: OccupancyGridMapUpdaterInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : Costmap2D( - cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) + : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } virtual ~OccupancyGridMapUpdaterInterface() = default; @@ -37,5 +38,6 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..50b470d904ef6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -45,6 +45,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -117,6 +119,8 @@ bool extractCommonPointCloud( sensor_msgs::msg::PointCloud2 & output_obstacle_pc); unsigned char getApproximateOccupancyState(const unsigned char & value); + } // namespace utils +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 62cfa4bcb5228..1edc538b3de3f 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): ), ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..ef40839d2a5aa 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -196,7 +196,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, remappings=[ diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 0e84e4860fdf3..bb5ef025a7e47 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ( diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 3d02be9ca7156..25eafcd564e2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -49,21 +49,23 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMap::OccupancyGridMap( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -140,7 +142,7 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r raytraceFreespace(pointcloud, robot_pose); // occupied - MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + MarkCell marker(costmap_, cost_value::LETHAL_OBSTACLE); for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y) { unsigned int mx, my; @@ -155,12 +157,12 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); + updateCellsByPointCloud(pointcloud, cost_value::FREE_SPACE); } void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); + updateCellsByPointCloud(pointcloud, cost_value::LETHAL_OBSTACLE); } void OccupancyGridMap::updateCellsByPointCloud( @@ -244,9 +246,10 @@ void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const P } constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + MarkCell marker(costmap_, cost_value::FREE_SPACE); raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 3d176e41583a0..5e9b5598c8f63 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -69,13 +69,16 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -230,3 +233,4 @@ void OccupancyGridMapInterface::raytrace( } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index d4e177209f99d..f25009d47bdcc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -33,6 +33,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -136,7 +139,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } // Second step: Add unknown cell @@ -162,9 +165,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); if (!no_freespace_point) { const auto & target = *raw_distance_iter; - raytrace( - source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); } continue; } @@ -177,7 +179,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } else if (no_freespace_point) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } @@ -186,13 +188,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -203,7 +205,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -215,7 +217,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= distance_margin_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -229,3 +231,4 @@ void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 03d7a42ae7043..a2400fb3c81b2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -34,6 +34,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -192,7 +195,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } if (pub_debug_grid_) @@ -219,7 +222,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); break; } @@ -227,7 +230,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); continue; } @@ -243,13 +246,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -262,7 +265,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -274,7 +277,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= obstacle_separation_threshold_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -299,3 +302,4 @@ void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 97% rename from perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp rename to perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 7e2c3f7ba304a..1e88b7a39fd14 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { @@ -61,9 +63,9 @@ namespace overwrite_fusion */ State getApproximateState(const unsigned char & occupancy) { - if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + if (occupancy >= cost_value::OCCUPIED_THRESHOLD) { return State::OCCUPIED; - } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + } else if (occupancy <= cost_value::FREE_THRESHOLD) { return State::FREE; } else { return State::UNKNOWN; @@ -320,3 +322,4 @@ unsigned char singleFrameOccupancyFusion( } } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 40f538a64f6e9..74506d095b36d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -48,15 +50,15 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( float pz{}; float not_pz{}; float po_hat{}; - if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + if (z == cost_value::LETHAL_OBSTACLE) { pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::FREE_SPACE) { + } else if (z == cost_value::FREE_SPACE) { pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::NO_INFORMATION) { + } else if (z == cost_value::NO_INFORMATION) { const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } @@ -79,3 +81,4 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 607694d6da571..9f3c3c7e40eaf 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -35,7 +37,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( using fusion_policy::convertProbabilityToChar; using fusion_policy::log_odds_fusion::logOddsFusion; - constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; /* Tau and ST decides how fast the observation decay to the unknown status*/ constexpr double tau = 0.75; @@ -68,4 +70,6 @@ bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupanc } return true; } + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp rename to perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 209207fe10f34..e8c2cdb2617df 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -189,13 +191,14 @@ bool extractCommonPointCloud( */ unsigned char getApproximateOccupancyState(const unsigned char & value) { - if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { - return occupancy_cost_value::LETHAL_OBSTACLE; - } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { - return occupancy_cost_value::FREE_SPACE; + if (value >= cost_value::OCCUPIED_THRESHOLD) { + return cost_value::LETHAL_OBSTACLE; + } else if (value <= cost_value::FREE_THRESHOLD) { + return cost_value::FREE_SPACE; } else { - return occupancy_cost_value::NO_INFORMATION; + return cost_value::NO_INFORMATION; } } } // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index d8ae702ef60ca..d8647ae2ba744 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -13,9 +13,9 @@ ament_cmake_auto autoware_cmake + eigen3_cmake_module autoware_universe_utils - eigen3_cmake_module grid_map_costmap_2d grid_map_msgs grid_map_ros @@ -27,12 +27,9 @@ rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros tf2_sensor_msgs - visualization_msgs pointcloud_preprocessor pointcloud_to_laserscan diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index fa93db802b0ce..a271e17a1e566 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" +#include "synchronized_grid_map_fusion_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; @@ -404,7 +404,7 @@ nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; costmap2d.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } @@ -424,7 +424,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; gridmap.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } return gridmap; @@ -453,14 +453,14 @@ nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsg msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); - unsigned char * data = occupancy_grid_map.getCharMap(); + const unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 85% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 84ca13c8b1881..c839f160aab9b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -45,7 +45,7 @@ // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -124,6 +124,6 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; }; -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 777c180fe1a3a..6c4ad8135b37d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; @@ -229,12 +229,12 @@ OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToM unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index a8adea6e700e5..a599f7b564c8b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapUpdaterInterface; @@ -100,6 +100,6 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index d019280aefda0..3fe75263b3f28 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "pointcloud_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -250,12 +250,12 @@ OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapTo unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 81% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..e0c7ef74988f4 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -38,7 +38,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapInterface; @@ -93,6 +93,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index c41c809a92b0c..f03852562f7ca 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the CostTranslationTable and InverseCostTranslationTable functions -using occupancy_cost_value::cost_translation_table; -using occupancy_cost_value::inverse_cost_translation_table; +using autoware::occupancy_grid_map::cost_value::cost_translation_table; +using autoware::occupancy_grid_map::cost_value::inverse_cost_translation_table; TEST(CostTranslationTableTest, TestRange) { diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 6b3dc8a2ebcef..78137371c720e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" + +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) { - using fusion_policy::log_odds_fusion::logOddsFusion; + using autoware::occupancy_grid_map::fusion_policy::log_odds_fusion::logOddsFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; @@ -49,7 +50,7 @@ TEST(FusionPolicyTest, TestLogOddsUpdateRule) // Test the dempster-shafer update rule TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) { - using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + using autoware::occupancy_grid_map::fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 4bc3dca0a67bd..17df981d849d4 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. // autoware -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -77,14 +77,14 @@ TEST(TestUtils, TestCropPointcloudByHeight) sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; // case1: normal input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); // case2: normal input, empty output - EXPECT_NO_THROW(utils::cropPointcloudByHeight( + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); // case3: empty input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); } From 0c2045904535f837cc5b2d01170adc68b6bb820c Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 8 Jul 2024 15:59:06 +0900 Subject: [PATCH 23/33] refactor(lanelet2_map_preprocessor): apply static analysis (#7870) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix * apply suggestion Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/fix_lane_change_tags.cpp | 33 +++---- .../src/fix_z_value_by_pcd.cpp | 43 ++++---- .../src/merge_close_lines.cpp | 98 ++++++++----------- .../src/merge_close_points.cpp | 23 ++--- .../src/remove_unreferenced_geometry.cpp | 27 ++--- .../src/transform_maps.cpp | 27 ++--- 6 files changed, 116 insertions(+), 135 deletions(-) diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index cbd279af2b46f..571c2e91c53a5 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -27,10 +27,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -48,32 +45,32 @@ bool loadLaneletMap( return true; } -lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; - for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { - lanelets.push_back(lanelet); - } + std::copy( + lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(), + std::back_inserter(lanelets)); return lanelets; } -void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lanelets = convertToVector(lanelet_map_ptr); - lanelet::traffic_rules::TrafficRulesPtr trafficRules = + auto lanelets = convert_to_vector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); - lanelet::routing::RoutingGraphUPtr routingGraph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + lanelet::routing::RoutingGraphUPtr routing_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules); for (auto & llt : lanelets) { - if (!routingGraph->conflicting(llt).empty()) { + if (!routing_graph->conflicting(llt).empty()) { continue; } llt.attributes().erase("turn_direction"); - if (!!routingGraph->adjacentRight(llt)) { + if (!!routing_graph->adjacentRight(llt)) { llt.rightBound().attributes()["lane_change"] = "yes"; } - if (!!routingGraph->adjacentLeft(llt)) { + if (!!routing_graph->adjacentLeft(llt)) { llt.leftBound().attributes()["lane_change"] = "yes"; } } @@ -91,11 +88,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - fixTags(llt_map_ptr); + fix_tags(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index 14a33b01beee0..af565208c5f71 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,16 +57,16 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -double getMinHeightAroundPoint( +double get_min_height_around_point( const pcl::PointCloud::Ptr & pcd_map_ptr, const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, const double search_radius3d, const double search_radius2d) { - std::vector pointIdxRadiusSearch; - std::vector pointRadiusSquaredDistance; + std::vector point_idx_radius_search; + std::vector point_radius_squared_distance; if ( kdtree.radiusSearch( - search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + search_pt, search_radius3d, point_idx_radius_search, point_radius_squared_distance) <= 0) { std::cout << "no points found within 3d radius " << search_radius3d << std::endl; return search_pt.z; } @@ -73,8 +74,7 @@ double getMinHeightAroundPoint( double min_height = std::numeric_limits::max(); bool found = false; - for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { - std::size_t pt_idx = pointIdxRadiusSearch.at(i); + for (auto pt_idx : point_idx_radius_search) { const auto pt = pcd_map_ptr->points.at(pt_idx); if (pt.z > min_height) { continue; @@ -91,8 +91,9 @@ double getMinHeightAroundPoint( return min_height; } -void adjustHeight( - const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +void adjust_height( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr) { std::unordered_set done; double search_radius2d = 0.5; @@ -107,11 +108,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -121,11 +122,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -148,14 +149,14 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - adjustHeight(pcd_map_ptr, llt_map_ptr); + adjust_height(pcd_map_ptr, llt_map_ptr); lanelet::write(llt_output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index e0d071ea11dc1..0e40d04a2cec3 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -26,10 +26,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -47,21 +44,17 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -lanelet::ConstPoint3d get3DPointFrom2DArcLength( +lanelet::ConstPoint3d get3d_point_from2d_arc_length( const lanelet::ConstLineString3d & line, const double s) { double accumulated_distance2d = 0; @@ -71,27 +64,23 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( auto prev_pt = line.front(); for (size_t i = 1; i < line.size(); i++) { const auto & pt = line[i]; - double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + double distance2d = + lanelet::geometry::distance2d(lanelet::utils::to2D(prev_pt), lanelet::utils::to2D(pt)); if (accumulated_distance2d + distance2d >= s) { double ratio = (s - accumulated_distance2d) / distance2d; auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; std::cout << interpolated_pt << std::endl; - return lanelet::ConstPoint3d( - lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + return lanelet::ConstPoint3d{ + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; } accumulated_distance2d += distance2d; prev_pt = pt; } RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); - return lanelet::ConstPoint3d(); -} - -double getLineLength(const lanelet::ConstLineString3d & line) -{ - return boost::geometry::length(line.basicLineString()); + return {}; } -bool areLinesSame( +bool are_lines_same( const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) { bool same_ends = false; @@ -105,66 +94,63 @@ bool areLinesSame( return false; } - double sum_distance = 0; - for (const auto & pt : line1) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line2); - } - for (const auto & pt : line2) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line1); - } + double sum_distance = + std::accumulate(line1.begin(), line1.end(), 0.0, [&line2](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line2); + }); + sum_distance += + std::accumulate(line2.begin(), line2.end(), 0.0, [&line1](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line1); + }); - double avg_distance = sum_distance / (line1.size() + line2.size()); + double avg_distance = sum_distance / static_cast(line1.size() + line2.size()); std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; - if (avg_distance < 1.0) { - return true; - } else { - return false; - } + return avg_distance < 1.0; } -lanelet::BasicPoint3d getClosestPointOnLine( +lanelet::BasicPoint3d get_closest_point_on_line( const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) { - auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + auto arc_coordinate = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(line), lanelet::utils::to2D(search_point)); std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; - return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); + return get3d_point_from2d_arc_length(line, arc_coordinate.length).basicPoint(); } -lanelet::LineString3d mergeTwoLines( +lanelet::LineString3d merge_two_lines( const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) { lanelet::Points3d new_points; for (const auto & p1 : line1) { - lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); - lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + const lanelet::BasicPoint3d & p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = get_closest_point_on_line(p1, line2); lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); new_points.push_back(new_point); } - return lanelet::LineString3d(lanelet::utils::getId(), new_points); + return lanelet::LineString3d{lanelet::utils::getId(), new_points}; } -void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +void copy_data(lanelet::LineString3d & dst, const lanelet::LineString3d & src) { - lanelet::Points3d points; dst.clear(); - for (lanelet::Point3d & pt : src) { - dst.push_back(pt); + for (const lanelet::ConstPoint3d & pt : src) { + dst.push_back(static_cast(pt)); } } -void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_lines(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + auto lines = convert_line_layer_to_line_strings(lanelet_map_ptr); for (size_t i = 0; i < lines.size(); i++) { auto line_i = lines.at(i); for (size_t j = 0; j < i; j++) { auto line_j = lines.at(j); - if (areLinesSame(line_i, line_j)) { - auto merged_line = mergeTwoLines(line_i, line_j); - copyData(line_i, merged_line); - copyData(line_j, merged_line); + if (are_lines_same(line_i, line_j)) { + auto merged_line = merge_two_lines(line_i, line_j); + copy_data(line_i, merged_line); + copy_data(line_j, merged_line); line_i.setId(line_j.id()); std::cout << line_j << " " << line_i << std::endl; // lanelet_map_ptr->add(merged_line); @@ -189,11 +175,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergeLines(llt_map_ptr); + merge_lines(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index e18a366003e10..3cbbffc702019 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,17 +43,12 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } @@ -72,9 +67,9 @@ lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_ma // return lanelet::LineString3d(lanelet::utils::getId(), new_points); // } -void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { - const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + const auto & points = convert_points_layer_to_points(lanelet_map_ptr); for (size_t i = 0; i < points.size(); i++) { auto point_i = points.at(i); @@ -109,11 +104,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergePoints(llt_map_ptr); + merge_points(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index 633dae4e20c1d..3ab10551e36f9 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,28 +43,29 @@ bool loadLaneletMap( return true; } -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +void remove_unreferenced_geometry(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); - for (auto llt : lanelet_map_ptr->laneletLayer) { + for (const auto & llt : lanelet_map_ptr->laneletLayer) { new_map->add(llt); } lanelet_map_ptr = new_map; @@ -82,10 +83,10 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - removeUnreferencedGeometry(llt_map_ptr); + remove_unreferenced_geometry(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index d29df716b11e7..d55e03c7faee5 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,9 +57,9 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -void transformMaps( - pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, - const Eigen::Affine3d affine) +void transform_maps( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const Eigen::Affine3d & affine) { { for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { @@ -74,14 +75,14 @@ void transformMaps( for (auto & pt : pcd_map_ptr->points) { Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); auto transformed_pt = affine * eigen_pt; - pt.x = transformed_pt.x(); - pt.y = transformed_pt.y(); - pt.z = transformed_pt.z(); + pt.x = static_cast(transformed_pt.x()); + pt.y = static_cast(transformed_pt.y()); + pt.z = static_cast(transformed_pt.z()); } } } -Eigen::Affine3d createAffineMatrixFromXYZRPY( +Eigen::Affine3d create_affine_matrix_from_xyzrpy( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -127,19 +128,19 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + Eigen::Affine3d affine = create_affine_matrix_from_xyzrpy(x, y, z, roll, pitch, yaw); const auto mgrs_grid = node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); std::cout << "using mgrs grid: " << mgrs_grid << std::endl; - transformMaps(pcd_map_ptr, llt_map_ptr, affine); + transform_maps(pcd_map_ptr, llt_map_ptr, affine); lanelet::write(llt_output_path, *llt_map_ptr, projector); pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); From deef7c4a484e62828e9e8ee2a3092db95fed9640 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 16:22:41 +0900 Subject: [PATCH 24/33] fix(pointcloud_preprocessor): fix the bug where the geometry message was not saved correctly. (#7886) * fix: fix bug that geometry message didn't save correctly Signed-off-by: vividf * chore: change some functions from public to protected Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- .../distortion_corrector.hpp | 36 +++++++++---------- .../distortion_corrector.cpp | 19 +++++----- 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 6f372f0e74646..8a7b7c90113da 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -61,7 +61,8 @@ class DistortionCorrectorBase template class DistortionCorrector : public DistortionCorrectorBase { -public: +protected: + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; @@ -72,28 +73,12 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque twist_queue_; std::deque angular_velocity_queue_; - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); + void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); void getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, @@ -105,6 +90,19 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; + +public: + explicit DistortionCorrector(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); }; class DistortionCorrector2D : public DistortionCorrector diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 81c29a9f6202a..8c8752c7e278e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -47,21 +47,20 @@ template void DistortionCorrector::processIMUMessage( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = - std::make_shared(); - getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); + getIMUTransformation(base_frame, imu_msg->header.frame_id); + enqueueIMU(imu_msg); } template void DistortionCorrector::getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) + const std::string & base_frame, const std::string & imu_frame) { if (imu_transform_exists_) { return; } + geometry_imu_to_base_link_ptr_ = std::make_shared(); + tf2::Transform tf2_imu_to_base_link; if (base_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); @@ -83,20 +82,18 @@ void DistortionCorrector::getIMUTransformation( } } - geometry_imu_to_base_link_ptr->transform.rotation = + geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } template -void DistortionCorrector::enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr); + tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_); transformed_angular_velocity.header = imu_msg->header; angular_velocity_queue_.push_back(transformed_angular_velocity); From 094f2e2a7ba4db09b591f526ef6d60df4937ff95 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:30:07 +0900 Subject: [PATCH 25/33] refactor(multi_object_tracker)!: fix namespace and directory structure (#7863) * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: move include files into 'autoware' Signed-off-by: Taekjin LEE * refactor: set namespace Signed-off-by: Taekjin LEE * refactor: refine logics to pass cppcheck Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 44 ++++++++++--------- .../association/association.hpp} | 14 +++--- .../association}/solver/gnn_solver.hpp | 12 ++--- .../solver/gnn_solver_interface.hpp | 10 +++-- .../association/solver/mu_ssp.hpp} | 12 +++-- .../association/solver/ssp.hpp} | 12 +++-- .../tracker/model/bicycle_tracker.hpp | 17 ++++--- .../tracker/model/big_vehicle_tracker.hpp | 17 ++++--- .../model/multiple_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/normal_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/pass_through_tracker.hpp | 11 +++-- .../model/pedestrian_and_bicycle_tracker.hpp | 17 ++++--- .../tracker/model/pedestrian_tracker.hpp | 19 ++++---- .../tracker/model/tracker_base.hpp | 13 ++++-- .../tracker/model/unknown_tracker.hpp | 15 ++++--- .../motion_model/bicycle_motion_model.hpp | 13 ++++-- .../motion_model/ctrv_motion_model.hpp | 14 +++--- .../tracker/motion_model/cv_motion_model.hpp | 13 ++++-- .../motion_model/motion_model_base.hpp | 11 +++-- .../tracker/object_model/object_model.hpp | 10 +++-- .../multi_object_tracker/tracker/tracker.hpp | 6 +-- .../multi_object_tracker/utils/utils.hpp | 6 +-- .../association/association.cpp} | 11 +++-- .../mu_successive_shortest_path/mu_ssp.cpp} | 6 ++- .../successive_shortest_path/ssp.cpp} | 6 ++- .../tracker/model/bicycle_tracker.cpp | 9 +++- .../tracker/model/big_vehicle_tracker.cpp | 8 +++- .../model/multiple_vehicle_tracker.cpp | 7 ++- .../tracker/model/normal_vehicle_tracker.cpp | 9 +++- .../tracker/model/pass_through_tracker.cpp | 9 +++- .../model/pedestrian_and_bicycle_tracker.cpp | 7 ++- .../tracker/model/pedestrian_tracker.cpp | 9 +++- .../tracker/model/tracker_base.cpp | 9 +++- .../tracker/model/unknown_tracker.cpp | 9 +++- .../motion_model/bicycle_motion_model.cpp | 11 +++-- .../motion_model/ctrv_motion_model.cpp | 10 +++-- .../tracker/motion_model/cv_motion_model.cpp | 10 +++-- .../motion_model/motion_model_base.cpp | 7 ++- .../src/debugger/debug_object.cpp | 9 +++- .../debugger/debug_object.hpp | 13 ++++-- .../src/debugger/debugger.cpp | 6 ++- .../debugger/debugger.hpp | 13 ++++-- ...core.cpp => multi_object_tracker_node.cpp} | 19 ++++---- .../multi_object_tracker_node.hpp} | 22 +++++----- .../src/processor/input_manager.cpp | 32 ++++++++------ .../processor/input_manager.hpp | 10 ++--- .../src/processor/processor.cpp | 9 +++- .../processor/processor.hpp | 12 +++-- 48 files changed, 398 insertions(+), 204 deletions(-) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/data_association.hpp => autoware/multi_object_tracker/association/association.hpp} (81%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver.hpp (55%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver_interface.hpp (73%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/mu_ssp.hpp} (71%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/ssp.hpp} (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/bicycle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pass_through_tracker.hpp (81%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/tracker_base.hpp (91%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/unknown_tracker.hpp (80%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp (87%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp (86%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/motion_model_base.hpp (85%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/object_model/object_model.hpp (96%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/tracker.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/utils/utils.hpp (98%) rename perception/multi_object_tracker/{src/data_association/data_association.cpp => lib/association/association.cpp} (96%) rename perception/multi_object_tracker/{src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp => lib/association/mu_successive_shortest_path/mu_ssp.cpp} (88%) rename perception/multi_object_tracker/{src/data_association/successive_shortest_path/successive_shortest_path.cpp => lib/association/successive_shortest_path/ssp.cpp} (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/bicycle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/big_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/multiple_vehicle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/normal_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pass_through_tracker.cpp (95%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_and_bicycle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/tracker_base.cpp (96%) rename perception/multi_object_tracker/{src => lib}/tracker/model/unknown_tracker.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/bicycle_motion_model.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/ctrv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/cv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/motion_model_base.cpp (93%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debug_object.hpp (91%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debugger.hpp (94%) rename perception/multi_object_tracker/src/{multi_object_tracker_core.cpp => multi_object_tracker_node.cpp} (96%) rename perception/multi_object_tracker/{include/multi_object_tracker/multi_object_tracker_core.hpp => src/multi_object_tracker_node.hpp} (82%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/input_manager.hpp (95%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/processor.hpp (91%) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 4f9268583dd34..370e5bb0b9161 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -20,32 +20,34 @@ include_directories( ) # Generate exe file -set(MULTI_OBJECT_TRACKER_SRC - src/multi_object_tracker_core.cpp +set(${PROJECT_NAME}_src + src/multi_object_tracker_node.cpp src/debugger/debugger.cpp src/debugger/debug_object.cpp src/processor/processor.cpp src/processor/input_manager.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/tracker/motion_model/motion_model_base.cpp - src/tracker/motion_model/bicycle_motion_model.cpp +) +set(${PROJECT_NAME}_lib + lib/association/association.cpp + lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/tracker/motion_model/motion_model_base.cpp + lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv - src/tracker/motion_model/ctrv_motion_model.cpp - src/tracker/motion_model/cv_motion_model.cpp - src/tracker/model/tracker_base.cpp - src/tracker/model/big_vehicle_tracker.cpp - src/tracker/model/normal_vehicle_tracker.cpp - src/tracker/model/multiple_vehicle_tracker.cpp - src/tracker/model/bicycle_tracker.cpp - src/tracker/model/pedestrian_tracker.cpp - src/tracker/model/pedestrian_and_bicycle_tracker.cpp - src/tracker/model/unknown_tracker.cpp - src/tracker/model/pass_through_tracker.cpp + lib/tracker/motion_model/ctrv_motion_model.cpp + lib/tracker/motion_model/cv_motion_model.cpp + lib/tracker/model/tracker_base.cpp + lib/tracker/model/big_vehicle_tracker.cpp + lib/tracker/model/normal_vehicle_tracker.cpp + lib/tracker/model/multiple_vehicle_tracker.cpp + lib/tracker/model/bicycle_tracker.cpp + lib/tracker/model/pedestrian_tracker.cpp + lib/tracker/model/pedestrian_and_bicycle_tracker.cpp + lib/tracker/model/unknown_tracker.cpp + lib/tracker/model/pass_through_tracker.cpp ) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${MULTI_OBJECT_TRACKER_SRC} + ${${PROJECT_NAME}_src} + ${${PROJECT_NAME}_lib} ) target_link_libraries(${PROJECT_NAME} @@ -54,8 +56,8 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "multi_object_tracker::MultiObjectTracker" - EXECUTABLE ${PROJECT_NAME}_node + PLUGIN "autoware::multi_object_tracker::MultiObjectTracker" + EXECUTABLE multi_object_tracker_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index c30423db02c26..2c12341d0aa67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include #include @@ -34,6 +34,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class DataAssociation { private: @@ -61,4 +63,6 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp similarity index 55% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp index 631fd73f8583d..3b72b800e86bc 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 73% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp index 1fb508bcf12b2..88f9181d96d9b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class GnnSolverInterface @@ -30,6 +32,8 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp index a4d9d727e6cc5..024670c3389c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class MuSSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp index 1c4b15cd68b59..0f987fc49ee7c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class SSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 63ad496b70ed9..5374e28d5f9cf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BicycleTracker : public Tracker { @@ -67,4 +70,6 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 7112e6a08ade1..c3f824aff35b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BigVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 75aec0b06d6b8..a0a6bd7781761 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -16,16 +16,19 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +namespace autoware::multi_object_tracker +{ + class MultipleVehicleTracker : public Tracker { private: @@ -48,4 +51,6 @@ class MultipleVehicleTracker : public Tracker virtual ~MultipleVehicleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 1165cecab258b..f7edebfc31378 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class NormalVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index cd661dab52c6e..e7f0a5fd699e1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yutaka Shimizu // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" +namespace autoware::multi_object_tracker +{ + class PassThroughTracker : public Tracker { private: @@ -44,4 +47,6 @@ class PassThroughTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 3c3ac038b085e..f593280c2e183 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +namespace autoware::multi_object_tracker +{ class PedestrianAndBicycleTracker : public Tracker { @@ -46,4 +49,6 @@ class PedestrianAndBicycleTracker : public Tracker virtual ~PedestrianAndBicycleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c5be57f656eb5..1e0f778a69137 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -16,15 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ class PedestrianTracker : public Tracker { @@ -49,7 +50,7 @@ class PedestrianTracker : public Tracker }; BoundingBox bounding_box_; Cylinder cylinder_; - + // cspell: ignore CTRV CTRVMotionModel motion_model_; using IDX = CTRVMotionModel::IDX; @@ -75,4 +76,6 @@ class PedestrianTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 44b3884c392e6..bc52e49f763fd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -33,6 +33,9 @@ #include +namespace autoware::multi_object_tracker +{ + class Tracker { private: @@ -114,4 +117,6 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp similarity index 80% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index ca8ecba160bd8..4bc03f439ffc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +namespace autoware::multi_object_tracker +{ class UnknownTracker : public Tracker { @@ -62,4 +65,6 @@ class UnknownTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 87% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index f6ce2842388c6..6e20d31aad168 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class BicycleMotionModel : public MotionModel { private: @@ -107,4 +110,6 @@ class BicycleMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 86% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 657048079c46c..3cca786d9f65b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,8 +32,10 @@ #endif #include -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ +// cspell: ignore CTRV class CTRVMotionModel : public MotionModel { private: @@ -92,4 +94,6 @@ class CTRVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 421e413d7aab7..ad3061285a80c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class CVMotionModel : public MotionModel { private: @@ -85,4 +88,6 @@ class CVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 85% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index e740612e96d4f..130053fafd2ed 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #include "kalman_filter/kalman_filter.hpp" @@ -31,6 +31,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class MotionModel { private: @@ -65,4 +68,6 @@ class MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp similarity index 96% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp index 6a771344bb36b..b49464109eec8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -46,6 +46,9 @@ constexpr T kmph2mps(const T kmph) } // namespace +namespace autoware::multi_object_tracker +{ + namespace object_model { @@ -301,5 +304,6 @@ static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); } // namespace object_model +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index a8bc58e254fc2..ea1e60d4c7e17 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/bicycle_tracker.hpp" #include "model/big_vehicle_tracker.hpp" @@ -29,4 +29,4 @@ #include "model/tracker_base.hpp" #include "model/unknown_tracker.hpp" -#endif // MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp similarity index 98% rename from perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 0bda7870ae2b9..833f768e171f4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include @@ -276,4 +276,4 @@ inline bool getMeasurementYaw( } // namespace utils -#endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/lib/association/association.cpp similarity index 96% rename from perception/multi_object_tracker/src/data_association/data_association.cpp rename to perception/multi_object_tracker/lib/association/association.cpp index f7b40cd178528..ac31cad4cc13c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/lib/association/association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/data_association.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,6 +68,9 @@ double getFormedYawAngle( } } // namespace +namespace autoware::multi_object_tracker +{ + DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, @@ -224,3 +227,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp similarity index 88% rename from perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp index 18779a7fffbdf..9ae6e300b2b9b 100644 --- a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" #include @@ -24,6 +24,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { void MuSSP::maximizeLinearAssignment( @@ -39,3 +41,5 @@ void MuSSP::maximizeLinearAssignment( solve_muSSP(cost, direct_assignment, reverse_assignment); } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp similarity index 98% rename from perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp index 0b52c09cb54b8..d20992c7d183b 100644 --- a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" #include #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { struct ResidualEdge @@ -368,3 +370,5 @@ void SSP::maximizeLinearAssignment( #endif } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index a438c830596d7..ba53ccec1ad43 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( @@ -334,3 +337,5 @@ bool BicycleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 90d33e65b46bd..0593b7fc9dc12 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,8 @@ #include #endif +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( @@ -398,3 +400,5 @@ bool BigVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 67445e5b0daa2..a6e5021e4fe7d 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool MultipleVehicleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index 67282893083c1..ef345692b32ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( @@ -399,3 +402,5 @@ bool NormalVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp similarity index 95% rename from perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 17be415480360..c872944fab3d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -16,10 +16,10 @@ // Author: v1.0 Yutaka Shimizu // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -36,6 +36,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -117,3 +120,5 @@ bool PassThroughTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 00609934990a3..8c665e6078a2b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool PedestrianAndBicycleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index d9412c3b5739c..ee50c2e5449ed 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( @@ -338,3 +341,5 @@ bool PedestrianTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp similarity index 96% rename from perception/multi_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp index f27bad172a4b9..daa8db5db91e6 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -14,9 +14,9 @@ // // -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -40,6 +40,9 @@ float decayProbability(const float & prior, const float & delta_time) } } // namespace +namespace autoware::multi_object_tracker +{ + Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification, @@ -202,3 +205,5 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 6905d7c3b8ced..0648007e0b807 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -13,13 +13,13 @@ // limitations under the License. #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -36,6 +36,9 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" +namespace autoware::multi_object_tracker +{ + UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -234,3 +237,5 @@ bool UnknownTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 466108a2bef0e..2e325d18579a6 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -17,17 +17,20 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ + // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity @@ -487,3 +490,5 @@ bool BicycleMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b19af1d26d162..b1c5a36f9ad5b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -17,17 +17,19 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -386,3 +388,5 @@ bool CTRVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 231ba73796ed9..2e2ba660a6e0d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -17,19 +17,21 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CV // Constant Velocity (CV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -306,3 +308,5 @@ bool CVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp index 4d51021de634b..ffbb452815fa7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +namespace autoware::multi_object_tracker +{ MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { @@ -90,3 +93,5 @@ bool MotionModel::getPredictedState( tmp_ekf_for_no_update.getP(P); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index fe6d3d5c75564..58b7dfc5cef48 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" @@ -48,6 +48,9 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) } } // namespace +namespace autoware::multi_object_tracker +{ + TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -79,7 +82,7 @@ void TrackerObjectDebugger::collect( const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { - if (!is_initialized_) is_initialized_ = true; + is_initialized_ = true; message_time_ = message_time; @@ -391,3 +394,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma return; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/src/debugger/debug_object.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp rename to perception/multi_object_tracker/src/debugger/debug_object.hpp index 08c9eb975d8e6..ed4c556959861 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#ifndef DEBUGGER__DEBUG_OBJECT_HPP_ +#define DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -37,6 +37,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + struct ObjectData { rclcpp::Time time; @@ -99,4 +102,6 @@ class TrackerObjectDebugger void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index e692ae76468e4..3e47bbe9bed8d 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debugger.hpp" +#include "debugger.hpp" #include +namespace autoware::multi_object_tracker +{ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) : node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { @@ -207,3 +209,5 @@ void TrackerDebugger::publishObjectsMarkers() // reset object data object_debugger_.reset(); } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/src/debugger/debugger.hpp similarity index 94% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp rename to perception/multi_object_tracker/src/debugger/debugger.hpp index a1c516147b220..77618e1882be7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/src/debugger/debugger.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#ifndef DEBUGGER__DEBUGGER_HPP_ +#define DEBUGGER__DEBUGGER_HPP_ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include #include @@ -33,6 +33,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + /** * @brief Debugger class for multi object tracker * @details This class is used to publish debug information of multi object tracker @@ -104,4 +107,6 @@ class TrackerDebugger void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp similarity index 96% rename from perception/multi_object_tracker/src/multi_object_tracker_core.cpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.cpp index 97c3d93d191b2..d3e1c9686b69b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -15,13 +15,12 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" +#include "multi_object_tracker_node.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include -#include #include @@ -66,7 +65,7 @@ boost::optional getTransformAnonymous( } // namespace -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -192,7 +191,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - data_association_ = std::make_unique( + association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); } @@ -295,9 +294,9 @@ void MultiObjectTracker::runProcess( const auto & list_tracker = processor_->getListTracker(); const auto & detected_objects = transformed_objects; // global nearest neighbor - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + association_->assign(score_matrix, direct_assignment, reverse_assignment); // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( @@ -358,6 +357,8 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->publishObjectsMarkers(); } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker + +#include -RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp similarity index 82% rename from perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.hpp index aff3cbd00eabe..db5eaaa88ca8c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp @@ -16,14 +16,14 @@ // Author: v1.0 Yukihiro Saito /// -#ifndef MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ -#define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#ifndef MULTI_OBJECT_TRACKER_NODE_HPP_ +#define MULTI_OBJECT_TRACKER_NODE_HPP_ -#include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger/debugger.hpp" -#include "multi_object_tracker/processor/input_manager.hpp" -#include "multi_object_tracker/processor/processor.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "debugger/debugger.hpp" +#include "processor/input_manager.hpp" +#include "processor/processor.hpp" #include @@ -52,7 +52,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObject = autoware_perception_msgs::msg::DetectedObject; @@ -82,7 +82,7 @@ class MultiObjectTracker : public rclcpp::Node // internal states std::string world_frame_id_; // tracking frame - std::unique_ptr data_association_; + std::unique_ptr association_; std::unique_ptr processor_; // input manager @@ -103,6 +103,6 @@ class MultiObjectTracker : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#endif // MULTI_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bee76e6c05940..5172cc5062450 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/input_manager.hpp" +#include "input_manager.hpp" #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { /////////////////////////// /////// InputStream /////// @@ -62,7 +62,7 @@ void InputStream::onMessage( { const DetectedObjects objects = *msg; objects_que_.push_back(objects); - if (objects_que_.size() > que_size_) { + while (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -160,21 +160,27 @@ void InputStream::getObjectsOlderThan( return; } - for (const auto & object : objects_que_) { - const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - - // remove objects older than the specified duration + for (const auto & objects : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(objects.header.stamp); + // ignore objects older than the specified duration if (object_time < object_oldest_time) { - objects_que_.pop_front(); continue; } // Add the object if the object is older than the specified latest time - if (object_latest_time >= object_time) { - std::pair object_pair(index_, object); - objects_list.push_back(object_pair); - // remove the object from the queue + if (object_time <= object_latest_time) { + std::pair objects_pair(index_, objects); + objects_list.push_back(objects_pair); + } + } + + // remove objects older than 'object_latest_time' + while (!objects_que_.empty()) { + const rclcpp::Time object_time = rclcpp::Time(objects_que_.front().header.stamp); + if (object_time < object_latest_time) { objects_que_.pop_front(); + } else { + break; } } } @@ -368,4 +374,4 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li return is_any_object; } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/src/processor/input_manager.hpp similarity index 95% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp rename to perception/multi_object_tracker/src/processor/input_manager.hpp index 29b0c18ae766f..432a4105d815f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/src/processor/input_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#ifndef PROCESSOR__INPUT_MANAGER_HPP_ +#define PROCESSOR__INPUT_MANAGER_HPP_ #include "rclcpp/rclcpp.hpp" @@ -26,7 +26,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; using ObjectsList = std::vector>; @@ -155,6 +155,6 @@ class InputManager void optimizeTimings(); }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#endif // PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 50d1a021c5838..17871c89b5258 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/processor.hpp" +#include "processor.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; TrackerProcessor::TrackerProcessor( @@ -248,3 +251,5 @@ void TrackerProcessor::getTentativeObjects( } } } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/src/processor/processor.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp rename to perception/multi_object_tracker/src/processor/processor.hpp index 6d0dbcd036e53..5eac113b3974c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/src/processor/processor.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +#ifndef PROCESSOR__PROCESSOR_HPP_ +#define PROCESSOR__PROCESSOR_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -29,6 +29,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class TrackerProcessor { public: @@ -78,4 +80,6 @@ class TrackerProcessor const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // PROCESSOR__PROCESSOR_HPP_ From f28ccd834427d2c7a07f3beef0fc3bc6edb2eb3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:34:56 +0900 Subject: [PATCH 26/33] refactor(detected_object_validation)!: fix namespace and directory structure (#7866) * refactor: update include paths for detected object filters Signed-off-by: Taekjin LEE * refactor: set proper namespace Signed-off-by: Taekjin LEE * refactor: move utils to be shared Signed-off-by: Taekjin LEE * refactor: remove unnecessary dependencies in detected_object_validation/package.xml Signed-off-by: Taekjin LEE * chore: Override functions in pcl_validator.hpp Signed-off-by: Taekjin LEE * style(pre-commit): autofix * refactor: pcl_validator to obstacle_pointcloud_validator, ogm_validator to occupancy_grid_map_validator Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../detected_object_validation/CMakeLists.txt | 28 +++++------- .../utils/utils.hpp | 10 +++-- .../{src => lib/utils}/utils.cpp | 6 ++- .../detected_object_validation/package.xml | 3 -- .../lanelet_filter.cpp} | 20 +++++---- .../lanelet_filter/lanelet_filter.hpp} | 27 ++++++----- .../obstacle_pointcloud}/debugger.hpp | 16 ++++--- .../obstacle_pointcloud_validator.cpp} | 14 +++--- .../obstacle_pointcloud_validator.hpp} | 45 ++++++++++++------- .../occupancy_grid_map_validator.cpp} | 21 +++++---- .../occupancy_grid_map_validator.hpp} | 19 +++++--- .../position_filter.cpp} | 12 +++-- .../position_filter/position_filter.hpp} | 23 +++++----- .../test_object_position_filter.cpp | 4 +- .../test/test_utils.cpp | 13 +++--- .../src/low_intensity_cluster_filter_node.hpp | 4 +- 16 files changed, 153 insertions(+), 112 deletions(-) rename perception/detected_object_validation/include/{ => autoware}/detected_object_validation/utils/utils.hpp (82%) rename perception/detected_object_validation/{src => lib/utils}/utils.cpp (87%) rename perception/detected_object_validation/src/{object_lanelet_filter.cpp => lanelet_filter/lanelet_filter.cpp} (95%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp => src/lanelet_filter/lanelet_filter.hpp} (81%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator => src/obstacle_pointcloud}/debugger.hpp (89%) rename perception/detected_object_validation/src/{obstacle_pointcloud_based_validator.cpp => obstacle_pointcloud/obstacle_pointcloud_validator.cpp} (97%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp => src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp} (79%) rename perception/detected_object_validation/src/{occupancy_grid_based_validator.cpp => occupancy_grid_map/occupancy_grid_map_validator.cpp} (92%) rename perception/detected_object_validation/{include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp => src/occupancy_grid_map/occupancy_grid_map_validator.hpp} (83%) rename perception/detected_object_validation/src/{object_position_filter.cpp => position_filter/position_filter.cpp} (91%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_position_filter.hpp => src/position_filter/position_filter.hpp} (71%) diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index c416209d7d55d..6d93e7b426ad2 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -28,12 +28,8 @@ include_directories( ) # Generate occupancy grid based validator exe file -set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC - src/occupancy_grid_based_validator.cpp -) - ament_auto_add_library(occupancy_grid_based_validator SHARED - ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC} + src/occupancy_grid_map/occupancy_grid_map_validator.cpp ) target_link_libraries(occupancy_grid_based_validator @@ -42,12 +38,8 @@ target_link_libraries(occupancy_grid_based_validator ) # Generate obstacle pointcloud based validator exe file -set(OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC - src/obstacle_pointcloud_based_validator.cpp -) - ament_auto_add_library(obstacle_pointcloud_based_validator SHARED - ${OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC} + src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp ) target_link_libraries(obstacle_pointcloud_based_validator @@ -56,32 +48,32 @@ target_link_libraries(obstacle_pointcloud_based_validator ) ament_auto_add_library(object_lanelet_filter SHARED - src/object_lanelet_filter.cpp - src/utils.cpp + src/lanelet_filter/lanelet_filter.cpp + lib/utils/utils.cpp ) ament_auto_add_library(object_position_filter SHARED - src/object_position_filter.cpp - src/utils.cpp + src/position_filter/position_filter.cpp + lib/utils/utils.cpp ) rclcpp_components_register_node(obstacle_pointcloud_based_validator - PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator" + PLUGIN "autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator" EXECUTABLE obstacle_pointcloud_based_validator_node ) rclcpp_components_register_node(object_lanelet_filter - PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode" + PLUGIN "autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode" EXECUTABLE object_lanelet_filter_node ) rclcpp_components_register_node(object_position_filter - PLUGIN "object_position_filter::ObjectPositionFilterNode" + PLUGIN "autoware::detected_object_validation::position_filter::ObjectPositionFilterNode" EXECUTABLE object_position_filter_node ) rclcpp_components_register_node(occupancy_grid_based_validator - PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + PLUGIN "autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator" EXECUTABLE occupancy_grid_based_validator_node ) diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp similarity index 82% rename from perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp rename to perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp index c55e992213eb5..3ec119d2a48c5 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include #include + +namespace autoware::detected_object_validation +{ namespace utils { struct FilterTargetLabel @@ -48,5 +51,6 @@ inline bool hasBoundingBox(const autoware_perception_msgs::msg::DetectedObject & } } // namespace utils +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#endif // AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/lib/utils/utils.cpp similarity index 87% rename from perception/detected_object_validation/src/utils.cpp rename to perception/detected_object_validation/lib/utils/utils.cpp index f6e2157cb885f..327f65a0c833e 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/lib/utils/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" #include +namespace autoware::detected_object_validation +{ namespace utils { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -27,4 +29,6 @@ bool FilterTargetLabel::isTarget(const uint8_t label) const (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } + } // namespace utils +} // namespace autoware::detected_object_validation diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index d8eb1257ba451..09440c9f6358d 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -22,17 +22,14 @@ autoware_perception_msgs autoware_test_utils autoware_universe_utils - geometry_msgs message_filters nav_msgs object_recognition_utils pcl_conversions rclcpp rclcpp_components - tf2 tf2_geometry_msgs tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp similarity index 95% rename from perception/detected_object_validation/src/object_lanelet_filter.cpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index d1ba34f4a61b2..09440fedc1764 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include "lanelet_filter.hpp" -#include -#include -#include -#include +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -25,7 +25,9 @@ #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_lanelet_filter_node", node_options), @@ -309,7 +311,9 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( return false; } -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_lanelet_filter::ObjectLaneletFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp similarity index 81% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 3ffd8d18c9a67..25d78a160c246 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef LANELET_FILTER__LANELET_FILTER_HPP_ +#define LANELET_FILTER__LANELET_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include -#include -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -33,7 +33,9 @@ #include #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; @@ -92,6 +94,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // LANELET_FILTER__LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp similarity index 89% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp index d338580d95418..5886474d51b34 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -12,19 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ +#define OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { class Debugger { @@ -106,6 +108,8 @@ class Debugger return pointcloud_xyz; } }; -} // namespace obstacle_pointcloud_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation + +#endif // OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp similarity index 97% rename from perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index fbb3f863be7c4..3fe527935c485 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "obstacle_pointcloud_validator.hpp" #include #include @@ -25,11 +27,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; @@ -369,8 +372,9 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( "debug/pipeline_latency_ms", pipeline_latency); } -} // namespace obstacle_pointcloud_based_validator +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation #include RCLCPP_COMPONENTS_REGISTER_NODE( - obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator) + autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp similarity index 79% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index a16bb63fd0c87..19b42ecf9055b 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -13,17 +13,17 @@ // limitations under the License. // NOLINTNEXTLINE(whitespace/line_length) -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#ifndef OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ // NOLINTNEXTLINE(whitespace/line_length) -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#define OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ -#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "debugger.hpp" -#include -#include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -42,7 +42,10 @@ #include #include #include -namespace obstacle_pointcloud_based_validator + +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { struct PointsNumThresholdParam @@ -91,14 +94,17 @@ class Validator2D : public Validator pcl::PointCloud::Ptr convertToXYZ( const pcl::PointCloud::Ptr & pointcloud_xy); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return convertToXYZ(neighbor_pointcloud_); } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -112,13 +118,16 @@ class Validator3D : public Validator public: explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return neighbor_pointcloud_; } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -154,7 +163,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); }; -} // namespace obstacle_pointcloud_based_validator + +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation // NOLINTNEXTLINE(whitespace/line_length) -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#endif // OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp similarity index 92% rename from perception/detected_object_validation/src/occupancy_grid_based_validator.cpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index b8682a6e56b3b..ce900f30f4255 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "occupancy_grid_map_validator.hpp" -#include -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "object_recognition_utils/object_classification.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -26,11 +28,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; @@ -173,7 +176,9 @@ void OccupancyGridBasedValidator::showDebugImage( cv::waitKey(2); } -} // namespace occupancy_grid_based_validator +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_based_validator::OccupancyGridBasedValidator) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp similarity index 83% rename from perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp index beb8faf5db1a3..52b6f79e5e20a 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ +#define OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include #include #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +31,9 @@ #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { class OccupancyGridBasedValidator : public rclcpp::Node { @@ -68,6 +71,8 @@ class OccupancyGridBasedValidator : public rclcpp::Node const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); }; -} // namespace occupancy_grid_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation + +#endif // OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/position_filter/position_filter.cpp similarity index 91% rename from perception/detected_object_validation/src/object_position_filter.cpp rename to perception/detected_object_validation/src/position_filter/position_filter.cpp index dccff8a6ccc67..ac0bab404c476 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "position_filter.hpp" -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_position_filter_node", node_options), @@ -78,7 +80,9 @@ bool ObjectPositionFilterNode::isObjectInBounds( position.y > lower_bound_y_ && position.y < upper_bound_y_; } -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_position_filter::ObjectPositionFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::position_filter::ObjectPositionFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/src/position_filter/position_filter.hpp similarity index 71% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/src/position_filter/position_filter.hpp index ea70d62fd952d..30a5d7fc9dfb1 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef POSITION_FILTER__POSITION_FILTER_HPP_ +#define POSITION_FILTER__POSITION_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +30,9 @@ #include #include -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { class ObjectPositionFilterNode : public rclcpp::Node @@ -57,6 +59,7 @@ class ObjectPositionFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // POSITION_FILTER__POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 03cebf91418ce..5bd174d5a9e17 100644 --- a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "../../src/position_filter/position_filter.hpp" #include #include @@ -21,10 +21,10 @@ #include +using autoware::detected_object_validation::position_filter::ObjectPositionFilterNode; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; -using object_position_filter::ObjectPositionFilterNode; std::shared_ptr generateTestManager() { diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp index f7077dadb1ef2..7865f928f23a8 100644 --- a/perception/detected_object_validation/test/test_utils.cpp +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include +using autoware::detected_object_validation::utils::FilterTargetLabel; using AutowareLabel = autoware_perception_msgs::msg::ObjectClassification; -utils::FilterTargetLabel createFilterTargetAll() +FilterTargetLabel createFilterTargetAll() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = true; filter.CAR = true; filter.TRUCK = true; @@ -34,9 +35,9 @@ utils::FilterTargetLabel createFilterTargetAll() return filter; } -utils::FilterTargetLabel createFilterTargetVehicle() +FilterTargetLabel createFilterTargetVehicle() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = false; filter.CAR = true; filter.TRUCK = true; diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 8e21306218ab2..61e23860cd195 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -15,9 +15,9 @@ #ifndef LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ #define LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ +#include "autoware/detected_object_validation/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "detected_object_validation/utils/utils.hpp" #include #include @@ -64,7 +64,7 @@ class LowIntensityClusterFilter : public rclcpp::Node // Eigen::Vector4f max_boundary_transformed_; bool is_validation_range_transformed_ = false; const std::string base_link_frame_id_ = "base_link"; - utils::FilterTargetLabel filter_target_; + autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; // debugger std::unique_ptr> stop_watch_ptr_{ From 1cbd11d789d12ba0bd771b2222d22476747d86a5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 8 Jul 2024 22:55:42 +0900 Subject: [PATCH 27/33] feat(autoware_universe_utils): add TimeKeeper to track function's processing time (#7754) Signed-off-by: Takayuki Murooka --- common/autoware_universe_utils/CMakeLists.txt | 25 ++ common/autoware_universe_utils/README.md | 249 ++++++++++++++++++ .../examples/example_scoped_time_track.cpp | 61 +++++ .../examples/example_time_keeper.cpp | 64 +++++ .../universe_utils/system/time_keeper.hpp | 202 ++++++++++++++ .../src/system/time_keeper.cpp | 175 ++++++++++++ .../test/src/system/test_time_keeper.cpp | 53 ++++ .../path_optimizer/common_structs.hpp | 52 ---- .../autoware/path_optimizer/mpt_optimizer.hpp | 5 +- .../include/autoware/path_optimizer/node.hpp | 7 +- .../state_equation_generator.hpp | 7 +- .../src/mpt_optimizer.cpp | 63 ++--- planning/autoware_path_optimizer/src/node.cpp | 71 +++-- .../src/state_equation_generator.cpp | 3 +- 14 files changed, 893 insertions(+), 144 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp create mode 100644 common/autoware_universe_utils/examples/example_time_keeper.cpp create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp create mode 100644 common/autoware_universe_utils/src/system/time_keeper.cpp create mode 100644 common/autoware_universe_utils/test/src/system/test_time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb60692..2fdeef2119ab8 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) + ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp @@ -16,6 +20,11 @@ ament_auto_add_library(autoware_universe_utils SHARED src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp src/system/backtrace.cpp + src/system/time_keeper.cpp +) + +target_link_libraries(autoware_universe_utils + fmt::fmt ) if(BUILD_TESTING) @@ -30,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b09635..2d24f84423299 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,252 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +## `autoware::universe_utils` + +### `systems` + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp new file mode 100644 index 0000000000000..010cc58aba8ae --- /dev/null +++ b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp @@ -0,0 +1,61 @@ +// 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/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 0000000000000..3f6037e68daac --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,64 @@ +// 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/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 0000000000000..96070f0f30b77 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,202 @@ +// 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__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ + +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message + */ + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::shared_ptr Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + tier4_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher with + * std_msgs::msg::String + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 0000000000000..58bed6677227c --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,175 @@ +// 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/universe_utils/system/time_keeper.hpp" + +#include +#include + +namespace autoware::universe_utils +{ + +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(name); + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + return new_child_node; +} + +std::string ProcessingTimeNode::to_string() const +{ + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + + std::ostringstream oss; + construct_string(*this, oss, "", true, true); + return oss.str(); +} + +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const +{ + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; +} + +std::shared_ptr ProcessingTimeNode::get_parent_node() const +{ + return parent_node_; +} +std::vector> ProcessingTimeNode::get_child_nodes() const +{ + return child_nodes_; +} +void ProcessingTimeNode::set_time(const double processing_time) +{ + processing_time_ = processing_time; +} +std::string ProcessingTimeNode::get_name() const +{ + return name_; +} + +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); +} + +void TimeKeeper::start_track(const std::string & func_name) +{ + if (current_time_node_ == nullptr) { + current_time_node_ = std::make_shared(func_name); + root_node_ = current_time_node_; + } else { + current_time_node_ = current_time_node_->add_child(func_name); + } + stop_watch_.tic(func_name); +} + +void TimeKeeper::end_track(const std::string & func_name) +{ + if (current_time_node_->get_name() != func_name) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but end_track({}) is called", + current_time_node_->get_name(), func_name)); + } + const double processing_time = stop_watch_.toc(func_name); + current_time_node_->set_time(processing_time); + current_time_node_ = current_time_node_->get_parent_node(); + + if (current_time_node_ == nullptr) { + report(); + } +} + +void TimeKeeper::report() +{ + if (current_time_node_ != nullptr) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but report() is called", current_time_node_->get_name())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) +{ + time_keeper_.start_track(func_name_); +} + +ScopedTimeTrack::~ScopedTimeTrack() +{ + time_keeper_.end_track(func_name_); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 0000000000000..71ca7cc74bec5 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,53 @@ +// 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/universe_utils/system/time_keeper.hpp" + +#include +#include + +#include + +#include +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::ScopedTimeTrack; + using autoware::universe_utils::TimeKeeper; + + rclcpp::Node node{"sample_node"}; + + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper(&std::cerr, publisher); + + ScopedTimeTrack st{"main_func", time_keeper}; + + { // funcA + ScopedTimeTrack st{"funcA", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + ScopedTimeTrack st{"funcB", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + ScopedTimeTrack st{"funcC", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } +} diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad95..399262f93e19d 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b4..3443ab663b642 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d29..6f75c649e02b2 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,10 +22,11 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -98,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d32..1d23d7788dca1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index b5fd266006a48..45943d7deec09 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -395,13 +395,13 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), traj_param_(traj_param), debug_data_ptr_(debug_data_ptr), - time_keeper_ptr_(time_keeper_ptr), + time_keeper_(time_keeper), logger_(node->get_logger().get_child("mpt_optimizer")) { // initialize mpt param @@ -411,7 +411,7 @@ MPTOptimizer::MPTOptimizer( // state equation generator state_equation_generator_ = - StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -520,8 +520,6 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 8. publish trajectories for debug publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - time_keeper_ptr_->toc(__func__, " "); - debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); prev_optimized_traj_points_ptr_ = @@ -541,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -549,14 +547,14 @@ std::vector MPTOptimizer::calcReferencePoints( const double backward_traj_length = traj_param_.output_backward_traj_length; // 1. resample and convert smoothed points type from trajectory points to reference points - time_keeper_ptr_->tic("resampleReferencePoints"); + time_keeper_->start_track("resampleReferencePoints"); auto ref_points = [&]() { const auto resampled_smoothed_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( smoothed_points, mpt_param_.delta_arc_length); return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - time_keeper_ptr_->toc("resampleReferencePoints", " "); + time_keeper_->end_track("resampleReferencePoints"); // 2. crop forward and backward with margin, and calculate spline interpolation // NOTE: Margin is added to calculate orientation, curvature, etc precisely. @@ -611,8 +609,6 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points.resize(mpt_param_.num_points); } - time_keeper_ptr_->toc(__func__, " "); - return ref_points; } @@ -639,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -681,8 +677,6 @@ void MPTOptimizer::updateFixedPoint(std::vector & ref_points) co ref_points.front().curvature = front_point.curvature; ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const @@ -697,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -786,8 +780,6 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -796,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -844,8 +836,6 @@ void MPTOptimizer::updateBounds( } } */ - - time_keeper_ptr_->toc(__func__, " "); return; } @@ -1104,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1160,8 +1150,6 @@ void MPTOptimizer::updateVehicleBounds( ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); } } - - time_keeper_ptr_->toc(__func__, " "); } // cost function: J = x' Q x + u' R u @@ -1169,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1228,7 +1216,6 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - time_keeper_ptr_->toc(__func__, " "); return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } @@ -1236,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1286,7 +1273,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1297,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1451,7 +1437,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1477,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1510,7 +1495,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const auto lower_bound = toStdVector(updated_const_mat.lower_bound); // initialize or update solver according to warm start - time_keeper_ptr_->tic("initOsqp"); + time_keeper_->start_track("initOsqp"); const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); @@ -1530,13 +1515,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( } prev_mat_n_ = H.rows(); prev_mat_m_ = A.rows(); - - time_keeper_ptr_->toc("initOsqp", " "); + time_keeper_->end_track("initOsqp"); // solve qp - time_keeper_ptr_->tic("solveOsqp"); + time_keeper_->start_track("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - time_keeper_ptr_->toc("solveOsqp", " "); + time_keeper_->end_track("solveOsqp"); // check solution status const int solution_status = std::get<3>(result); @@ -1563,8 +1547,6 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::VectorXd optimized_variables = Eigen::Map(&optimization_result[0], N_v); - time_keeper_ptr_->toc(__func__, " "); - if (u0) { // manual warm start return static_cast(optimized_variables + *u0); } @@ -1647,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1700,7 +1682,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1708,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( @@ -1723,8 +1704,6 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); - - time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 3899867a9dcce..7c73d1ad1fee3 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -23,6 +23,7 @@ #include "rclcpp/time.hpp" #include +#include #include namespace autoware::path_optimizer @@ -38,7 +39,8 @@ std::vector concatVectors(const std::vector & prev_vector, const std::vect return concatenated_vector; } -StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +[[maybe_unused]] StringStamped createStringStamped( + const rclcpp::Time & now, const std::string & data) { StringStamped msg; msg.stamp = now; @@ -46,7 +48,7 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } -Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +[[maybe_unused]] Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) { Float64Stamped msg; msg.stamp = now; @@ -85,8 +87,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()), - time_keeper_ptr_(std::make_shared()) + debug_data_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -102,6 +103,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -120,8 +124,6 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); - time_keeper_ptr_->enable_calculation_time_info = - declare_parameter("option.debug.enable_calculation_time_info"); vehicle_stop_margin_outside_drivable_area_ = declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); @@ -133,11 +135,14 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); + // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, - time_keeper_ptr_); + time_keeper_); // reset planners // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been @@ -177,9 +182,6 @@ rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( - parameters, "option.debug.enable_calculation_time_info", - time_keeper_ptr_->enable_calculation_time_info); updateParam( parameters, "common.vehicle_stop_margin_outside_drivable_area", @@ -220,8 +222,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_ptr_->init(); - time_keeper_ptr_->tic(__func__); + time_keeper_->start_track(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -267,21 +268,21 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); - time_keeper_ptr_->toc(__func__, ""); - *time_keeper_ptr_ << "========================================"; - time_keeper_ptr_->endLine(); - // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time + /* const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); debug_calculation_time_str_pub_->publish(calculation_time_msg); debug_calculation_time_float_pub_->publish( createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); + */ const auto output_traj_msg = autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); + + time_keeper_->end_track(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -319,7 +320,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -339,13 +340,12 @@ std::vector PathOptimizer::generateOptimizedTrajectory( // 4. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); - time_keeper_ptr_->toc(__func__, " "); return optimized_traj_points; } std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +372,6 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - time_keeper_ptr_->toc(__func__, " "); return mpt_traj; } @@ -391,7 +390,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,14 +485,12 @@ void PathOptimizer::applyInputVelocity( trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -553,13 +550,11 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( } else { debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -569,36 +564,33 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos } virtual_wall_pub_->publish(virtual_wall_marker); - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!enable_pub_debug_marker_) { return; } - time_keeper_ptr_->tic(__func__); - // debug marker - time_keeper_ptr_->tic("getDebugMarker"); + time_keeper_->start_track("getDebugMarker"); const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); - time_keeper_ptr_->toc("getDebugMarker", " "); + time_keeper_->end_track("getDebugMarker"); - time_keeper_ptr_->tic("publishDebugMarker"); + time_keeper_->start_track("publishDebugMarker"); debug_markers_pub_->publish(debug_marker); - time_keeper_ptr_->toc("publishDebugMarker", " "); - - time_keeper_ptr_->toc(__func__, " "); + time_keeper_->end_track("publishDebugMarker"); } std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -654,20 +646,17 @@ std::vector PathOptimizer::extendTrajectory( // debug_data_ptr_->extended_traj_points = // extended_traj_points ? *extended_traj_points : std::vector(); - time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } void PathOptimizer::publishDebugData(const Header & header) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); - - time_keeper_ptr_->toc(__func__, " "); } } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5aa2ab4ffbfa8..74033c5834db2 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -60,7 +60,6 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( W.segment(i * D_x, D_x) = Wd; } - time_keeper_ptr_->toc(__func__, " "); return Matrix{A, B, W}; } From a70aa0e55d8debc49b4d39f9118ef72c6697fe3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:29:58 +0900 Subject: [PATCH 28/33] fix(multi_object_tracker): fix publish interval adjust timing (#7904) refactor: optimize publish time check in multi_object_tracker_node.cpp Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/multi_object_tracker_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index d3e1c9686b69b..198d9e2238566 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -221,8 +221,9 @@ void MultiObjectTracker::onTrigger() } else { // Publish if the next publish time is close const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(current_time); + const rclcpp::Time publish_time = this->now(); + if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(publish_time); } } } From 7dcdd676f55f4c24893a589da0e543a2d7a72a97 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:55:53 +0900 Subject: [PATCH 29/33] refactor(tier4_perception_launch): add maintainer to tier4_perception_launch (#7893) refactor: add maintainer to tier4_perception_launch Signed-off-by: Taekjin LEE --- launch/tier4_perception_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 57d4b209efeef..5c15a937304ad 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -7,6 +7,7 @@ Yukihiro Saito Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto From 6d0247e078d2e944c482b78a95eac9025f5863c2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 11:24:09 +0900 Subject: [PATCH 30/33] refactor(euclidean_cluster)!: fix namespace and directory structure (#7887) * refactor: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE refactor: fix namespace Signed-off-by: Taekjin LEE chore: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debugger/debugger.hpp | 3 -- .../src/detection_by_tracker_node.cpp | 4 +-- .../src/detection_by_tracker_node.hpp | 8 +++--- perception/euclidean_cluster/CMakeLists.txt | 28 +++++++++---------- .../euclidean_cluster/euclidean_cluster.hpp | 8 +++--- .../euclidean_cluster_interface.hpp | 4 +-- .../euclidean_cluster/utils.hpp | 4 +-- .../voxel_grid_based_euclidean_cluster.hpp | 8 +++--- .../launch/euclidean_cluster.launch.py | 4 +-- ...xel_grid_based_euclidean_cluster.launch.py | 4 +-- .../lib/euclidean_cluster.cpp | 6 ++-- perception/euclidean_cluster/lib/utils.cpp | 6 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 6 ++-- .../src/euclidean_cluster_node.cpp | 8 +++--- .../src/euclidean_cluster_node.hpp | 7 +++-- ...oxel_grid_based_euclidean_cluster_node.cpp | 8 +++--- ...oxel_grid_based_euclidean_cluster_node.hpp | 6 ++-- .../src/roi_pointcloud_fusion/node.cpp | 4 +-- 18 files changed, 62 insertions(+), 64 deletions(-) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster.hpp (87%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster_interface.hpp (95%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/utils.hpp (94%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp (90%) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 56f37fb10043d..bda42aa5f0b55 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -17,9 +17,6 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c22d015aa11a4..4c65500d96aaa 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -115,7 +115,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) setMaxSearchRange(); shape_estimator_ = std::make_shared(true, true); - cluster_ = std::make_shared( + cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = @@ -277,7 +277,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( const auto & label = target_object.classification.front().label; // initialize clustering parameters - euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + autoware::euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); // convert to pcl diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 095708fb4b51f..9c974d72cfdca 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,12 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" @@ -69,7 +69,7 @@ class DetectionByTracker : public rclcpp::Node TrackerHandler tracker_handler_; std::shared_ptr shape_estimator_; - std::shared_ptr cluster_; + std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index fe1fa9fda5fca..dacbcf460a14a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -13,45 +13,45 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(cluster_lib SHARED - lib/utils.cpp +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/euclidean_cluster.cpp lib/voxel_grid_based_euclidean_cluster.cpp + lib/utils.cpp ) -target_link_libraries(cluster_lib +target_link_libraries(${PROJECT_NAME}_lib ${PCL_LIBRARIES} ) -target_include_directories(cluster_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -ament_auto_add_library(euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_node_core SHARED src/euclidean_cluster_node.cpp ) -target_link_libraries(euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(euclidean_cluster_node_core - PLUGIN "euclidean_cluster::EuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_core + PLUGIN "autoware::euclidean_cluster::EuclideanClusterNode" EXECUTABLE euclidean_cluster_node ) -ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_voxel_grid_node_core SHARED src/voxel_grid_based_euclidean_cluster_node.cpp ) -target_link_libraries(voxel_grid_based_euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_voxel_grid_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core - PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core + PLUGIN "autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" EXECUTABLE voxel_grid_based_euclidean_cluster_node ) diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 87% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp index aec3f56936828..8c4aad537cc43 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp @@ -14,14 +14,14 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanCluster : public EuclideanClusterInterface { @@ -42,4 +42,4 @@ class EuclideanCluster : public EuclideanClusterInterface float tolerance_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 95% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp index 65b907f747666..461d75d4931db 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanClusterInterface { @@ -54,4 +54,4 @@ class EuclideanClusterInterface int max_cluster_size_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 94% rename from perception/euclidean_cluster/include/euclidean_cluster/utils.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp index 50d2306d48f72..a52c0840c4d41 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); void convertPointCloudClusters2Msg( @@ -37,4 +37,4 @@ void convertPointCloudClusters2Msg( void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 90% rename from perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index 375cc2d19a01f..5c51bb85ce177 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -14,15 +14,15 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface { @@ -52,4 +52,4 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface int min_points_number_per_voxel_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index b8d4f61a9cf28..053438a4352b0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", "low_height/pointcloud"), @@ -60,7 +60,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 607e1bf30ccaa..0b502b1c43d67 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", "low_height/pointcloud"), @@ -61,7 +61,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 5ba1b99403280..0481b7e1b742d 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() { @@ -93,4 +93,4 @@ bool EuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6d119ef3d01aa..624c838ef0647 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -11,7 +11,7 @@ // 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 "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include @@ -19,7 +19,7 @@ #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { @@ -128,4 +128,4 @@ void convertObjectMsg2SensorMsg( output.height = 1; output.is_dense = false; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 126f877afddb0..096de3dc1b296 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() { @@ -166,4 +166,4 @@ bool VoxelGridBasedEuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index f986b0334935f..b0b9448dfc0c4 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) : Node("euclidean_cluster_node", options) @@ -86,8 +86,8 @@ void EuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 92f10696d17dc..5ab48abb4f042 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include @@ -26,7 +26,8 @@ #include #include -namespace euclidean_cluster + +namespace autoware::euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node { @@ -45,4 +46,4 @@ class EuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e9425095a3b0d..e54c55e4873ee 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "voxel_grid_based_euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( const rclcpp::NodeOptions & options) @@ -89,8 +89,8 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b0c5d0e5a7fbf..b48e30b37de04 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include @@ -26,7 +26,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node { @@ -45,4 +45,4 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 7f19402d9e565..904e66cb53298 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -25,7 +25,7 @@ #include #endif -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" namespace image_projection_based_fusion { @@ -68,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { sensor_msgs::msg::PointCloud2 debug_cluster_msg; - euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } From 03877b072f7d58f5c42fc06a33d2b330941acd3b Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 9 Jul 2024 13:42:29 +0900 Subject: [PATCH 31/33] chore(radar_object_tracker): delete maintainer (#7898) Signed-off-by: scepter914 --- perception/radar_object_tracker/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 79ae0385d4b5e..f104e2d7ea456 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -6,7 +6,6 @@ Do tracking radar object Yoshi Ri Yukihiro Saito - Satoshi Tanaka Taekjin Lee Apache License 2.0 From 458dbe5c5445fe95a62d9a4804509acbc024d703 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 9 Jul 2024 07:45:46 +0200 Subject: [PATCH 32/33] chore(autoware_behavior_velocity_planner): remove no_prefix function from tests (#7589) Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3482d7be8ec48..cc2f4bf3b96b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,7 +54,6 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); From f716b5b0ac4678d5d75ae06936d66e340292930d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 15:02:25 +0900 Subject: [PATCH 33/33] fix(euclidean_cluster): include autoware_universe_utils headers (#7908) fix: include autoware_universe_utils headers instead of compare_map_segmentation package Signed-off-by: Taekjin LEE --- perception/euclidean_cluster/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index c5cdd9c54b988..2644786ead061 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_perception_msgs - compare_map_segmentation + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions