From d5a6c54cc57a98911fc199680746b779fdcd5aee Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 31 May 2024 15:36:22 +0900 Subject: [PATCH 01/23] feat(lane_change): apply new RTC state (#7152) * feat(lane_change): support for new RTC state transition Signed-off-by: Fumiya Watanabe * fix: distance at aborting Signed-off-by: Fumiya Watanabe * fix(lane_change): empty check Signed-off-by: Fumiya Watanabe * fix(rtc_interface):update activation depends on RTC state Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../interface.hpp | 12 ++++++++++ .../src/interface.cpp | 22 +++++++++++++++---- .../src/utils/utils.cpp | 10 +++++++-- planning/rtc_interface/src/rtc_interface.cpp | 22 +++++++++++++++---- 4 files changed, 56 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 6e40f880467fd..4c3d129ac687e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -125,6 +125,18 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; + void updateRTCStatus( + const double start_distance, const double finish_distance, const bool safe, + const uint8_t & state) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now()); + } + } + } + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 2567787a3f2e4..1669842117f9f 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -117,13 +117,18 @@ BehaviorModuleOutput LaneChangeInterface::plan() updateSteeringFactorPtr(output); if (module_type_->isAbortState()) { waitApproval(); - removeRTCStatus(); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::ABORTING); } else { clearWaitingApproval(); + const auto path = + assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); } return output; @@ -147,7 +152,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { - removeRTCStatus(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); path_candidate_ = std::make_shared(); return out; } @@ -156,7 +163,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; return out; @@ -239,6 +247,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } @@ -249,6 +260,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isAbleToReturnCurrentLane()) { log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } } diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index ab9a0b7176fdb..4eccd00d78c09 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -646,8 +646,14 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path) { - const auto start_idx = path.info.shift_line.start_idx; - const auto end_idx = path.info.shift_line.end_idx; + if (path.shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto start_idx = + std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1); + const auto end_idx = + std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1); return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index ea80dc9bb9d1f..77fe29b41d893 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -153,7 +153,16 @@ std::vector RTCInterface::validateCooperateCommands( registered_status_.statuses.begin(), registered_status_.statuses.end(), [command](auto & s) { return s.uuid == command.uuid; }); if (itr != registered_status_.statuses.end()) { - response.success = true; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[validateCooperateCommands] uuid : " + << to_string(command.uuid) + << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " + << itr->state.type << std::endl); + response.success = false; + } } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) @@ -175,8 +184,10 @@ void RTCInterface::updateCooperateCommandStatus(const std::vectorcommand_status = command.command; - itr->auto_mode = false; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + itr->command_status = command.command; + itr->auto_mode = false; + } } } } @@ -219,7 +230,7 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.state.type = State::WAITING_FOR_EXECUTION; + status.state.type = state; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; @@ -291,6 +302,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const [uuid](auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { + if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { + return false; + } if (itr->auto_mode) { return itr->safe; } else { From c92185ae76acbe22a505efaf83533c6377a7f5a6 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 31 May 2024 16:00:16 +0900 Subject: [PATCH 02/23] chore(motion_velocity_smoother): remove string stream creation (#7131) * chore(motion_velocity_smoother): remove string stream creation Signed-off-by: Fumiya Watanabe * fix: use format string Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../velocity_planning_utils.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 2bcb31ff10969..9fa4d697f06cb 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -199,13 +199,12 @@ bool calcStopVelocityWithConstantJerkAccLimit( } // for debug - std::stringstream ss; + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity."); for (unsigned int i = 0; i < ts.size(); ++i) { - ss << "t: " << ts.at(i) << ", x: " << xs.at(i) << ", v: " << vs.at(i) << ", a: " << as.at(i) - << ", j: " << js.at(i) << std::endl; + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "--- t: %f, x: %f, v: %f, a: %f, j: %f", + ts.at(i), xs.at(i), vs.at(i), as.at(i), js.at(i)); } - RCLCPP_DEBUG( - rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity. %s", ss.str().c_str()); const double a_target = 0.0; const double v_margin = 0.3; From fea4ed982b6b36b0ad37d803262af6c22520e8c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 31 May 2024 16:05:34 +0900 Subject: [PATCH 03/23] fix(avoidance): fix bugs in parked vehicle filtering logic (#7072) * fix(avoidance): check prev/next lane Signed-off-by: satoshi-ota * fix(avoidance): fix parked vehicle filtering Signed-off-by: satoshi-ota * fix(avoidance): check intersection location Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 95 ++++++++++++++++--- 1 file changed, 80 insertions(+), 15 deletions(-) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 1533a7dd49edf..660b8d228b6f2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -318,25 +318,55 @@ bool isWithinCrosswalk( bool isWithinIntersection( const ObjectData & object, const std::shared_ptr & route_handler) { - const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else"); + if (area_id == "else") { + return false; + } + + const std::string location = object.overhang_lanelet.attributeOr("location", "else"); + if (location == "private") { return false; } const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); - const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } -bool isOnEgoLane(const ObjectData & object) +bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - return boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), - object.overhang_lanelet.polygon2d().basicPolygon()); + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon())) { + return true; + } + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + prev_lanelet.front().polygon2d().basicPolygon())) { + return true; + } + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + next_lanelet.polygon2d().basicPolygon())) { + return true; + } + } + + return false; } bool isParallelToEgoLane(const ObjectData & object, const double threshold) @@ -604,14 +634,49 @@ bool isNeverAvoidanceTarget( if (object.is_on_ego_lane) { const auto right_lane = - planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + const lanelet::Attribute & right_lane_sub_type = + right_lane.value().attribute(lanelet::AttributeName::Subtype); + if (right_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_right_lane = + boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_right_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + } + const auto left_lane = - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); - if (right_lane.has_value() && left_lane.has_value()) { - object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; - RCLCPP_DEBUG( - rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); - return true; + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + const lanelet::Attribute & left_lane_sub_type = + left_lane.value().attribute(lanelet::AttributeName::Subtype); + if (left_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_left_lane = + boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_left_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } } } @@ -754,7 +819,7 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & parameters) { object.behavior = getObjectBehavior(object, parameters); - object.is_on_ego_lane = isOnEgoLane(object); + object.is_on_ego_lane = isOnEgoLane(object, planner_data->route_handler); if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { return false; From 693244601f3e67c88bb4eca52ee38fb323c59934 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:10:14 +0900 Subject: [PATCH 04/23] feat(ar_tag_based_localizer): componentize ArTagBasedLocalizer (#7187) * remove unusing main func Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change exec name and change log output from log to both 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> --- .../ar_tag_based_localizer/CMakeLists.txt | 20 ++++--- .../launch/ar_tag_based_localizer.launch.xml | 2 +- .../ar_tag_based_localizer/package.xml | 1 + .../src/ar_tag_based_localizer.cpp | 5 +- .../ar_tag_based_localizer/src/main.cpp | 53 ------------------- 5 files changed, 19 insertions(+), 62 deletions(-) delete mode 100644 localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index d625064b8f6cb..a8435fa056847 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,27 +14,33 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -ament_auto_add_executable(ar_tag_based_localizer - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/ar_tag_based_localizer.cpp ) -target_include_directories(ar_tag_based_localizer + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) -target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ArTagBasedLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_ar_tag_based_localizer + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/ar_tag_based_localizer.cpp ) - target_include_directories(test_ar_tag_based_localizer + target_include_directories(test_${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) - target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) + target_link_libraries(test_${PROJECT_NAME} ${OpenCV_LIBRARIES}) endif() ament_auto_package( diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 272338905c3f0..34602ca70daf4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 072479cc7aaf5..90fdd2fee31f4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -24,6 +24,7 @@ lanelet2_extension localization_util rclcpp + rclcpp_components tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 43ac1e1098453..9a6823e330acd 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -65,7 +65,7 @@ #include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) -: Node("ar_tag_based_localizer", options), cam_info_received_(false) +: rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) { /* Declare node parameters @@ -346,3 +346,6 @@ std::vector ArTagBasedLocalizer::detect_landmarks( return landmarks; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ArTagBasedLocalizer) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp deleted file mode 100644 index 8ef1dd6195580..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr ptr = std::make_shared(); - rclcpp::spin(ptr); - rclcpp::shutdown(); -} From 05e307118df971027992570576cd2fb8c709517e Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 31 May 2024 16:35:02 +0900 Subject: [PATCH 05/23] feat(obstacle_cruise_planner)!: ignore to garze against unknwon objects (#7177) Signed-off-by: Yuki Takagi Co-authored-by: Takayuki Murooka --- .../config/obstacle_cruise_planner.param.yaml | 3 ++- .../include/obstacle_cruise_planner/node.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 23 ++++++++++++++----- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index bd9c5750af78f..2ccd000657948 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -87,7 +87,8 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: -0.3 # lateral margin between the unknown obstacles and ego's footprint crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 3f18c6e1728c6..fd65446408db1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -200,6 +200,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double prediction_resampling_time_horizon; // max lateral margin double max_lat_margin_for_stop; + double max_lat_margin_for_stop_against_unknown; double max_lat_margin_for_cruise; double max_lat_margin_for_slow_down; double lat_hysteresis_margin_for_slow_down; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 21e3ecb7bf758..c5f0c73a13b78 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -247,6 +247,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara max_lat_margin_for_stop = node.declare_parameter("behavior_determination.stop.max_lat_margin"); + max_lat_margin_for_stop_against_unknown = + node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); max_lat_margin_for_cruise = node.declare_parameter("behavior_determination.cruise.max_lat_margin"); enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); @@ -311,6 +313,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin_against_unknown", + max_lat_margin_for_stop_against_unknown); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( @@ -682,8 +687,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( }(); const double max_lat_margin = std::max( - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), - p.max_lat_margin_for_slow_down); + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); if (max_lat_margin < min_lat_dist_to_traj_poly) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1174,7 +1179,13 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } - if (p.max_lat_margin_for_stop < precise_lat_dist) { + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { return std::nullopt; } @@ -1207,7 +1218,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + std::hypot( vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1232,8 +1243,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = + createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); From b3c83d50e1eb0d1849e7ef54fefaddb9e78ab1ca Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:36:08 +0900 Subject: [PATCH 06/23] feat(pose_estimator_arbiter): componentize PoseEstimatorArbiter (#7183) * mod to componentize and use glog Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau * style(pre-commit): autofix * Update localization/pose_estimator_arbiter/CMakeLists.txt add namespace Co-authored-by: Kento Yabuuchi * remove unusing main func 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> Co-authored-by: Kento Yabuuchi --- .../pose_estimator_arbiter/CMakeLists.txt | 25 ++++++-------- .../example_rule/CMakeLists.txt | 4 +-- .../launch/pose_estimator_arbiter.launch.xml | 2 +- .../pose_estimator_arbiter/package.xml | 5 +-- .../pose_estimator_arbiter.hpp | 2 +- .../pose_estimator_arbiter_core.cpp | 7 ++-- .../pose_estimator_arbiter_node.cpp | 33 ------------------- 7 files changed, 20 insertions(+), 58 deletions(-) delete mode 100644 localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt index 9a47b654a6ab4..eefb7fc9a6879 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -4,29 +4,23 @@ project(pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(glog REQUIRED) - find_package(PCL REQUIRED COMPONENTS common) include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) -# ============================== -# switch rule library -ament_auto_add_library(switch_rule - SHARED - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp -) -target_include_directories(switch_rule PUBLIC src) - # ============================== # pose estimator arbiter node -ament_auto_add_executable(${PROJECT_NAME} +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) -target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) -# ============================== +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +47,7 @@ endif() add_subdirectory(example_rule) # ============================== -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch ) diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt index 333f92842b860..b2b5a828e42e7 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -7,7 +7,7 @@ ament_auto_add_library(example_rule src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule switch_rule) +target_link_libraries(example_rule pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" switch_rule example_rule) + target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index 0a708e3f48988..b5be96fc3ce44 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 480b323f3031d..d164086ada87e 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -15,12 +15,12 @@ autoware_auto_mapping_msgs diagnostic_msgs geometry_msgs - glog lanelet2_extension magic_enum pcl_conversions pcl_ros pluginlib + rclcpp_components sensor_msgs std_msgs std_srvs @@ -29,9 +29,6 @@ visualization_msgs yabloc_particle_filter - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index 9e67dfc063964..54dac6ac254c1 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -47,7 +47,7 @@ class PoseEstimatorArbiter : public rclcpp::Node using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: - PoseEstimatorArbiter(); + explicit PoseEstimatorArbiter(const rclcpp::NodeOptions & options); private: // Set of running pose estimators specified by ros param `pose_sources` diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 4fc3fd9b914a6..67c555227976d 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -42,8 +42,8 @@ static std::unordered_set parse_estimator_name_args( return running_estimator_list; } -PoseEstimatorArbiter::PoseEstimatorArbiter() -: Node("pose_estimator_arbiter"), +PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), logger_configure_(std::make_unique(this)) @@ -211,3 +211,6 @@ void PoseEstimatorArbiter::on_timer() } } // namespace pose_estimator_arbiter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp deleted file mode 100644 index 20aaaf10abaab..0000000000000 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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 "pose_estimator_arbiter/pose_estimator_arbiter.hpp" - -#include - -int main(int argc, char * argv[]) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} From 633f9b26f012dd5b9f1a5479da50477aa03e3272 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:49:42 +0900 Subject: [PATCH 07/23] feat(map_height_fitter): componentize MapHeightFitter (#7192) * rename file name and mod to componentize and use glog Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change exec name and add log output both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- map/map_height_fitter/CMakeLists.txt | 12 ++++++++---- .../launch/map_height_fitter.launch.xml | 2 +- map/map_height_fitter/package.xml | 1 + .../src/{node.cpp => map_height_fitter_node.cpp} | 15 ++++----------- 4 files changed, 14 insertions(+), 16 deletions(-) rename map/map_height_fitter/src/{node.cpp => map_height_fitter_node.cpp} (83%) diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index 8821158c54757..0dec2f6a1663a 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -5,8 +5,9 @@ find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) autoware_package() -ament_auto_add_library(map_height_fitter SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp + src/map_height_fitter_node.cpp ) target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) @@ -14,11 +15,14 @@ target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) # These are treated as errors in compile, so pedantic warnings are disabled for this package. target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) -ament_auto_add_executable(node - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapHeightFitterNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor ) ament_auto_package( INSTALL_TO_SHARE launch - config) + config +) diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 353f2151ee172..3e01a35a8e519 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 7f384aa43ec7b..568c77f2509c6 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -25,6 +25,7 @@ libpcl-common pcl_conversions rclcpp + rclcpp_components sensor_msgs tf2_geometry_msgs tf2_ros diff --git a/map/map_height_fitter/src/node.cpp b/map/map_height_fitter/src/map_height_fitter_node.cpp similarity index 83% rename from map/map_height_fitter/src/node.cpp rename to map/map_height_fitter/src/map_height_fitter_node.cpp index 55702dc08450d..fdc7604b68855 100644 --- a/map/map_height_fitter/src/node.cpp +++ b/map/map_height_fitter/src/map_height_fitter_node.cpp @@ -23,7 +23,8 @@ using tier4_localization_msgs::srv::PoseWithCovarianceStamped; class MapHeightFitterNode : public rclcpp::Node { public: - MapHeightFitterNode() : Node("map_height_fitter"), fitter_(this) + explicit MapHeightFitterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("map_height_fitter", options), fitter_(this) { const auto on_service = [this]( const PoseWithCovarianceStamped::Request::SharedPtr req, @@ -46,13 +47,5 @@ class MapHeightFitterNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_; }; -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) From 38ccce7a81183ab9962159442004496e963c8ece Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 31 May 2024 18:24:40 +0900 Subject: [PATCH 08/23] feat(diagnostic_graph_utils): componentize node (#7189) Signed-off-by: Takagi, Isamu --- system/diagnostic_graph_utils/CMakeLists.txt | 13 ++++++++++--- .../diagnostic_graph_utils/doc/node/converter.md | 4 ++-- system/diagnostic_graph_utils/doc/node/dump.md | 4 ++-- system/diagnostic_graph_utils/package.xml | 1 + .../diagnostic_graph_utils/src/node/converter.cpp | 15 +++------------ .../diagnostic_graph_utils/src/node/converter.hpp | 2 +- system/diagnostic_graph_utils/src/node/dump.cpp | 15 +++------------ system/diagnostic_graph_utils/src/node/dump.hpp | 2 +- 8 files changed, 23 insertions(+), 33 deletions(-) diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index 48af5e8fc304f..b68e7bedb57ed 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -9,12 +9,19 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/lib/subscription.cpp ) -ament_auto_add_executable(dump +ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp + src/node/converter.cpp ) -ament_auto_add_executable(converter - src/node/converter.cpp +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::DumpNode" + EXECUTABLE dump_node +) + +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::ConverterNode" + EXECUTABLE converter_node ) ament_auto_package() diff --git a/system/diagnostic_graph_utils/doc/node/converter.md b/system/diagnostic_graph_utils/doc/node/converter.md index c3db547167474..407a99c87f73e 100644 --- a/system/diagnostic_graph_utils/doc/node/converter.md +++ b/system/diagnostic_graph_utils/doc/node/converter.md @@ -5,7 +5,7 @@ This tool converts `/diagnostics_graph` to `/diagnostics_array` so it can be rea ## Usage ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` Terminal 3: diff --git a/system/diagnostic_graph_utils/doc/node/dump.md b/system/diagnostic_graph_utils/doc/node/dump.md index 090e9679676b6..c76bb85ed75cb 100644 --- a/system/diagnostic_graph_utils/doc/node/dump.md +++ b/system/diagnostic_graph_utils/doc/node/dump.md @@ -5,7 +5,7 @@ This tool displays `/diagnostics_graph` in table format. ## Usage ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` Output: diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml index 42d44efb6697e..c5a70363bfecb 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/diagnostic_graph_utils/package.xml @@ -12,6 +12,7 @@ diagnostic_msgs rclcpp + rclcpp_components tier4_system_msgs ament_lint_auto diff --git a/system/diagnostic_graph_utils/src/node/converter.cpp b/system/diagnostic_graph_utils/src/node/converter.cpp index e9e40fa0a0756..159cc6e0c3cab 100644 --- a/system/diagnostic_graph_utils/src/node/converter.cpp +++ b/system/diagnostic_graph_utils/src/node/converter.cpp @@ -19,7 +19,7 @@ namespace diagnostic_graph_utils { -ConverterNode::ConverterNode() : Node("converter") +ConverterNode::ConverterNode(const rclcpp::NodeOptions & options) : Node("converter", options) { using std::placeholders::_1; pub_array_ = create_publisher("/diagnostics_array", rclcpp::QoS(1)); @@ -40,14 +40,5 @@ void ConverterNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::ConverterNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::ConverterNode) diff --git a/system/diagnostic_graph_utils/src/node/converter.hpp b/system/diagnostic_graph_utils/src/node/converter.hpp index 09ce62d7293ec..19364a8ff8240 100644 --- a/system/diagnostic_graph_utils/src/node/converter.hpp +++ b/system/diagnostic_graph_utils/src/node/converter.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class ConverterNode : public rclcpp::Node { public: - ConverterNode(); + explicit ConverterNode(const rclcpp::NodeOptions & options); private: using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; diff --git a/system/diagnostic_graph_utils/src/node/dump.cpp b/system/diagnostic_graph_utils/src/node/dump.cpp index 5fa4922dec1a5..8456a92cbaa9b 100644 --- a/system/diagnostic_graph_utils/src/node/dump.cpp +++ b/system/diagnostic_graph_utils/src/node/dump.cpp @@ -24,7 +24,7 @@ namespace diagnostic_graph_utils { -DumpNode::DumpNode() : Node("dump") +DumpNode::DumpNode(const rclcpp::NodeOptions & options) : Node("dump", options) { using std::placeholders::_1; sub_graph_.register_create_callback(std::bind(&DumpNode::on_create, this, _1)); @@ -132,14 +132,5 @@ void DumpNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::DumpNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::DumpNode) diff --git a/system/diagnostic_graph_utils/src/node/dump.hpp b/system/diagnostic_graph_utils/src/node/dump.hpp index e37a1ea971bf5..c990fb77a53db 100644 --- a/system/diagnostic_graph_utils/src/node/dump.hpp +++ b/system/diagnostic_graph_utils/src/node/dump.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class DumpNode : public rclcpp::Node { public: - DumpNode(); + explicit DumpNode(const rclcpp::NodeOptions & options); private: void on_create(DiagGraph::ConstSharedPtr graph); From 9ae42389e978fe4743e2a1e53b478a70236be53c Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 31 May 2024 18:25:11 +0900 Subject: [PATCH 09/23] feat(dummy_diag_publisher): componentize node (#7190) Signed-off-by: Takagi, Isamu --- system/dummy_diag_publisher/CMakeLists.txt | 8 ++++-- .../dummy_diag_publisher_core.hpp | 2 +- .../dummy_diag_publisher_node.launch.xml | 2 +- .../src/dummy_diag_publisher_core.cpp | 17 +++++++----- .../src/dummy_diag_publisher_node.cpp | 26 ------------------- 5 files changed, 19 insertions(+), 36 deletions(-) delete mode 100644 system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt index cecb317bf9c34..794e7d35e1194 100644 --- a/system/dummy_diag_publisher/CMakeLists.txt +++ b/system/dummy_diag_publisher/CMakeLists.txt @@ -4,11 +4,15 @@ project(dummy_diag_publisher) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/dummy_diag_publisher_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/dummy_diag_publisher_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "DummyDiagPublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp index 8398c15b8e8f6..071e665ece6ec 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp +++ b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp @@ -35,7 +35,7 @@ struct DiagConfig class DummyDiagPublisher : public rclcpp::Node { public: - DummyDiagPublisher(); + explicit DummyDiagPublisher(const rclcpp::NodeOptions & options); private: enum Status { diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml index 8e71ce37543c2..9d9193fb5f7a7 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index cfe1692c91df9..9abf325e62833 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -245,12 +245,14 @@ void DummyDiagPublisher::onTimer() updater_.force_update(); } -DummyDiagPublisher::DummyDiagPublisher() -: Node( - "dummy_diag_publisher", rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)), - updater_(this) +rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) +{ + return options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( + true); +} + +DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) +: Node("dummy_diag_publisher", override_options(options)), updater_(this) { // Parameter @@ -277,3 +279,6 @@ DummyDiagPublisher::DummyDiagPublisher() timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&DummyDiagPublisher::onTimer, this)); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(DummyDiagPublisher) diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp deleted file mode 100644 index a532e8d1c6d01..0000000000000 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2020 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 "dummy_diag_publisher/dummy_diag_publisher_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} From ba292be31fcb069081ae0cb56a265989a8e59969 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 31 May 2024 18:37:39 +0900 Subject: [PATCH 10/23] refactor(start_planner): improve readability of end condition (#7181) * refactor(start_planner): clarify condition of transition to success Signed-off-by: kyoichi-sugahara * modify flowchart Signed-off-by: kyoichi-sugahara * add debug print Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../README.md | 76 +++++++++++++------ .../start_planner_module.hpp | 3 +- .../src/start_planner_module.cpp | 48 +++++++++--- 3 files changed, 92 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ff0550e4d4867..dd3dddef9d863 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -91,36 +91,66 @@ The `StartPlannerModule` is designed to initiate its execution based on specific ### **End Conditions** -The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution. +The `StartPlannerModule` terminates when specific conditions are met, depending on the type of planner being used. The `canTransitSuccessState` function determines whether the module should transition to the success state based on the following criteria: -```plantuml -@startuml -start -:Start hasFinishedPullOut(); +#### When the Freespace Planner is active -if (status_.driving_forward && status_.found_pull_out_path) then (yes) +- If the end point of the freespace path is reached, the module transitions to the success state. - if (status_.planner_type == FREESPACE) then (yes) - :Calculate distance\nto pull_out_path.end_pose; - if (distance < th_arrived_distance) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - else (no) - :Calculate arclength for\ncurrent_pose and pull_out_path.end_pose; - if (arclength_current - arclength_pull_out_end > offset) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - endif +#### When any other type of planner is active + +The transition to the success state is determined by the following conditions: + +- If a reverse path is being generated or the search for a pull out path fails: + - The module does not transition to the success state. +- If the end point of the pull out path's shift section is reached: + - The module transitions to the success state. + +The flowchart below illustrates the decision-making process in the `canTransitSuccessState` function: +```plantuml +@startuml +@startuml +skinparam ActivityBackgroundColor #white +skinparam ActivityBorderColor #black +skinparam ActivityBorderThickness 1 +skinparam ActivityArrowColor #black +skinparam ActivityArrowThickness 1 +skinparam ActivityStartColor #black +skinparam ActivityEndColor #black +skinparam ActivityDiamondBackgroundColor #white +skinparam ActivityDiamondBorderColor #black +skinparam ActivityDiamondFontColor #black +partition canTransitSuccessState() { +start +if (planner type is FREESPACE?) then (yes) +if (Has reached freespace end?) then (yes) +#FF006C:true; +stop else (no) - :return false;\n(Continue running); +:false; +stop endif - +else (no) +if (driving is forward?) then (yes) +if (pull out path is found?) then (yes) +if (Has reached pull out end?) then (yes) +#FF006C:true; stop +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +endif +} @enduml ``` diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 591d8adb60819..ea5afb133d57f 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -306,7 +306,8 @@ class StartPlannerModule : public SceneModuleInterface const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; - bool hasFinishedPullOut() const; + bool hasReachedFreespaceEnd() const; + bool hasReachedPullOutEnd() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 280e1112ef7fb..d3afdb2aefae5 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -579,7 +579,35 @@ bool StartPlannerModule::isExecutionReady() const bool StartPlannerModule::canTransitSuccessState() { - return hasFinishedPullOut(); + // Freespace Planner: + // - Can transit to success if the goal position is reached. + // - Cannot transit to success if the goal position is not reached. + if (status_.planner_type == PlannerType::FREESPACE) { + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; + } + + // Other Planners: + // - Cannot transit to success if the vehicle is driving in reverse. + // - Cannot transit to success if a safe path cannot be found due to: + // - Insufficient margin against static objects. + // - No path found that stays within the lane. + // In such cases, a stop point needs to be embedded and keep running start_planner. + // - Can transit to success if the end point of the pullout path is reached. + if (!status_.driving_forward || !status_.found_pull_out_path) { + return false; + } + + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan() @@ -1180,17 +1208,16 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( return stop_objects_in_pull_out_lanes; } -bool StartPlannerModule::hasFinishedPullOut() const +bool StartPlannerModule::hasReachedFreespaceEnd() const { - if (!status_.driving_forward || !status_.found_pull_out_path) { - return false; - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; +} +bool StartPlannerModule::hasReachedPullOutEnd() const +{ const auto current_pose = planner_data_->self_odometry->pose.pose; - if (status_.planner_type == PlannerType::FREESPACE) { - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < - parameters_->th_arrived_distance; - } // check that ego has passed pull out end point const double backward_path_length = @@ -1205,9 +1232,8 @@ bool StartPlannerModule::hasFinishedPullOut() const // offset to not finish the module before engage constexpr double offset = 0.1; - const bool has_finished = arclength_current.length - arclength_pull_out_end.length > offset; - return has_finished; + return arclength_current.length - arclength_pull_out_end.length > offset; } bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const From 76ae2cdd8b7f6325d7ca361c638cbec5626af30b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 31 May 2024 19:34:00 +0900 Subject: [PATCH 11/23] fix(ndt_scan_matcher): change diag time_stamp (#7195) * change diag time_stamp Signed-off-by: Yamato Ando * fix time now Signed-off-by: Yamato Ando * style(pre-commit): autofix * rename variable Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ndt_scan_matcher/diagnostics_module.hpp | 5 ++-- .../src/diagnostics_module.cpp | 9 ++++--- .../src/map_update_module.cpp | 2 -- .../src/ndt_scan_matcher_core.cpp | 26 ++++++++++++------- 4 files changed, 25 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp index 828eb6bed346b..4f7b5eff6521b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -31,10 +31,11 @@ class DiagnosticsModule template void addKeyValue(const std::string & key, const T & value); void updateLevelAndMessage(const int8_t level, const std::string & message); - void publish(); + void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; rclcpp::Publisher::SharedPtr diagnostics_pub_; diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 9d7eed46414ea..1e08ebb89f51e 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -85,15 +85,16 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str } } -void DiagnosticsModule::publish() +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray()); + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; - diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.header.stamp = publish_time_stamp; diagnostics_msg.status.push_back(diagnostics_status_msg_); if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index ae68dfc44c2c9..40f0b1f4465fa 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,8 +52,6 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().nanoseconds()); - // check is_activated diagnostics_ptr->addKeyValue("is_activated", is_activated); if (!is_activated) { 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 4da1c6865dbb7..8acfe3bd5c1ca 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -193,11 +193,15 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) void NDTScanMatcher::callback_timer() { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_map_update_->clear(); + diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", ros_time_now.nanoseconds()); + map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); - diagnostics_map_update_->publish(); + diagnostics_map_update_->publish(ros_time_now); } void NDTScanMatcher::callback_initial_pose( @@ -207,7 +211,7 @@ void NDTScanMatcher::callback_initial_pose( callback_initial_pose_main(initial_pose_msg_ptr); - diagnostics_initial_pose_->publish(); + diagnostics_initial_pose_->publish(initial_pose_msg_ptr->header.stamp); } void NDTScanMatcher::callback_initial_pose_main( @@ -260,7 +264,7 @@ void NDTScanMatcher::callback_regularization_pose( regularization_pose_buffer_->push_back(pose_conv_msg_ptr); - diagnostics_regularization_pose_->publish(); + diagnostics_regularization_pose_->publish(pose_conv_msg_ptr->header.stamp); } void NDTScanMatcher::callback_sensor_points( @@ -286,7 +290,7 @@ void NDTScanMatcher::callback_sensor_points( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_scan_points_->publish(); + diagnostics_scan_points_->publish(sensor_points_msg_in_sensor_frame->header.stamp); } bool NDTScanMatcher::callback_sensor_points_main( @@ -867,8 +871,10 @@ void NDTScanMatcher::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -878,15 +884,19 @@ void NDTScanMatcher::service_trigger_node( diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); - diagnostics_trigger_node_->publish(); + diagnostics_trigger_node_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_ndt_align_->clear(); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); + service_ndt_align_main(req, res); // check is_succeed_service @@ -899,15 +909,13 @@ void NDTScanMatcher::service_ndt_align( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_ndt_align_->publish(); + diagnostics_ndt_align_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); - // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; From ecdbb651e1786baf770dae1e0f5ce957b9ae0c95 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 20:39:15 +0900 Subject: [PATCH 12/23] fix(map_tf_generator): add log output (#7197) add log output Signed-off-by: a-maumau --- map/map_tf_generator/launch/map_tf_generator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index 43d487c2bc982..197085f31d9c4 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -3,7 +3,7 @@ - + From 7970b4f2339d868c3ca67c334e944bd674605665 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 21:35:03 +0900 Subject: [PATCH 13/23] fix(gnss_poser): change log output from screen to both (#7200) change log output from screen to both Signed-off-by: a-maumau --- sensing/gnss_poser/launch/gnss_poser.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 90c3553312dc4..c33f9a7589812 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -7,7 +7,7 @@ - + From 833509668ccff949dc61d32665d91fa89d7f9ae7 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 31 May 2024 21:48:06 +0900 Subject: [PATCH 14/23] refactor(image_projection_based_fusion): rework param (#6289) --- ...roi_sync.param.yaml => fusion_common.param.yaml} | 0 .../segmentation_pointcloud_fusion.param.yaml | 13 ++++++------- .../launch/pointpainting_fusion.launch.xml | 2 +- .../launch/roi_cluster_fusion.launch.xml | 2 +- .../launch/roi_detected_object_fusion.launch.xml | 2 +- .../launch/roi_pointcloud_fusion.launch.xml | 2 +- .../segmentation_pointcloud_fusion.launch.xml | 2 +- ...i_sync.schema.json => fusion_common.schema.json} | 4 ++-- 8 files changed, 13 insertions(+), 14 deletions(-) rename perception/image_projection_based_fusion/config/{roi_sync.param.yaml => fusion_common.param.yaml} (100%) rename perception/image_projection_based_fusion/schema/{roi_sync.schema.json => fusion_common.schema.json} (97%) diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/fusion_common.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/roi_sync.param.yaml rename to perception/image_projection_based_fusion/config/fusion_common.param.yaml 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 79a8b860ebdd3..2120a909cd672 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 @@ -31,10 +31,9 @@ # debug debug_mode: false - filter_scope_min_x: -100 - filter_scope_max_x: 100 - filter_scope_min_y: -100 - filter_scope_max_y: 100 - filter_scope_min_z: -100 - filter_scope_max_z: 100 - image_buffer_size: 15 + 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 diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index a22b2c2a13bda..d2f803f13d376 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -24,7 +24,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 8df1a374b00b6..f624b099fccb3 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -19,7 +19,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index c9da81af9ddb0..f11280b7f7c67 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -19,7 +19,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index 046d88d06e2a1..cde06744aca58 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -21,7 +21,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml index 1db2bb20793ac..cf4d104b9e05a 100644 --- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -18,7 +18,7 @@ - + diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/fusion_common.schema.json similarity index 97% rename from perception/image_projection_based_fusion/schema/roi_sync.schema.json rename to perception/image_projection_based_fusion/schema/fusion_common.schema.json index 411fb678a49a7..73ee1661adaea 100644 --- a/perception/image_projection_based_fusion/schema/roi_sync.schema.json +++ b/perception/image_projection_based_fusion/schema/fusion_common.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Synchronization of RoI Fusion Nodes", "type": "object", "definitions": { - "roi_sync": { + "fusion_common": { "type": "object", "properties": { "input_offset_ms": { @@ -74,7 +74,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/roi_sync" + "$ref": "#/definitions/fusion_common" } }, "required": ["ros__parameters"] From 6fe9fc648eaa3c1c36dc75fb8800212bd075bbfe Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Sat, 1 Jun 2024 08:06:08 +0900 Subject: [PATCH 15/23] feat(start_planner,lane_departure_checker): add margin param for lane departure check (#7193) * add param for lane departure margin Signed-off-by: Daniel Sanchez * json thing Signed-off-by: Daniel Sanchez * docs Signed-off-by: Daniel Sanchez * make separate param for lane departure margin expansion Signed-off-by: Daniel Sanchez * update docs Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/lane_departure_checker/README.md | 1 + .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker.hpp | 1 + .../schema/lane_departure_checker.json | 6 ++++++ .../lane_departure_checker.cpp | 3 ++- .../lane_departure_checker_node.cpp | 2 ++ .../README.md | 17 +++++++++-------- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 5 +++++ .../src/start_planner_module.cpp | 5 +++++ 11 files changed, 34 insertions(+), 9 deletions(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 67b770aefb3d2..4a6592a103f2f 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -93,6 +93,7 @@ This package includes the following features: | Name | Type | Description | Default value | | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | | footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| footprint_extra_margin | double | Coefficient for expanding footprint margin. When checking for lane departure | 0.0 | | resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | | max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | | delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index f0a8df21c425b..2724c28e2536a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -17,6 +17,7 @@ # Core footprint_margin_scale: 1.0 + footprint_extra_margin: 0.0 resample_interval: 0.3 max_deceleration: 2.8 delay_time: 1.3 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index c658cf4497973..b95c0a4b26e5c 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -60,6 +60,7 @@ typedef boost::geometry::index::rtree LaneDepartureChecker::createVehicleFootprints( const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(); + const auto local_vehicle_footprint = + vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); // Create vehicle footprint on each Path point std::vector vehicle_footprints; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 06d11133920f4..4d6c86990c399 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -150,6 +150,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Core Parameter param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); + param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); param_.resample_interval = declare_parameter("resample_interval"); param_.max_deceleration = declare_parameter("max_deceleration"); param_.delay_time = declare_parameter("delay_time"); @@ -403,6 +404,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin); update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index dd3dddef9d863..ab2f0460b3fcb 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -494,14 +494,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 46469de68149e..7de76c28cef5c 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -40,6 +40,7 @@ geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 + lane_departure_check_expansion_margin: 0.0 backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg # search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 0e85c1d0e76b3..04248ee7bd5fb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -71,6 +71,7 @@ struct StartPlannerParameters behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; + double lane_departure_check_expansion_margin{0.0}; // shift pull out bool enable_shift_pull_out{false}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 794577d7cc7aa..d5731f54224b6 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -101,6 +101,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg p.parallel_parking_parameters.center_line_path_interval = @@ -435,6 +437,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); updateParam( parameters, ns + "pull_out_max_steer_angle", p->parallel_parking_parameters.pull_out_max_steer_angle); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index d3afdb2aefae5..7e1f38f30c378 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,6 +65,11 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + + lane_departure_checker_->setParam(lane_departure_checker_params); // set enabled planner if (parameters_->enable_shift_pull_out) { From d70c8c97fb0120af2dcc08ab96866449e7ce07fa Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 2 Jun 2024 10:52:21 +0900 Subject: [PATCH 16/23] feat(obstacle_avoidance_planner): use polling subscriber (#7213) Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner/node.hpp | 12 ++++---- .../obstacle_avoidance_planner/src/node.cpp | 30 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 3677e6c5fb075..db17496288766 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -22,6 +22,7 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -83,16 +84,14 @@ class ObstacleAvoidancePlanner : public rclcpp::Node TrajectoryParam traj_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr virtual_wall_pub_; // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -113,8 +112,9 @@ class ObstacleAvoidancePlanner : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; - PlannerData createPlannerData(const Path & path) const; + bool checkInputPath(const Path & path, rclcpp::Clock clock) const; + PlannerData createPlannerData( + const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const; std::vector generateOptimizedTrajectory(const PlannerData & planner_data); std::vector extendTrajectory( const std::vector & traj_points, diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index c1da988eb5c25..b05d3f9da0c57 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -95,8 +95,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -224,8 +222,16 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); - // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + // check if input path is valid + if (!checkInputPath(*path_ptr, *get_clock())) { + return; + } + + // check if ego's odometry is valid + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + if (!ego_odom_ptr) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist."); return; } @@ -245,7 +251,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) } // 1. create planner data - const auto planner_data = createPlannerData(*path_ptr); + const auto planner_data = createPlannerData(*path_ptr, ego_odom_ptr); // 2. generate optimized trajectory const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); @@ -276,13 +282,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } -bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); - return false; - } - if (path.points.size() < 2) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); return false; @@ -297,7 +298,8 @@ bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock cloc return true; } -PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const +PlannerData ObstacleAvoidancePlanner::createPlannerData( + const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { // create planner data PlannerData planner_data; @@ -305,8 +307,8 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); planner_data.left_bound = path.left_bound; planner_data.right_bound = path.right_bound; - planner_data.ego_pose = ego_state_ptr_->pose.pose; - planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; + planner_data.ego_pose = ego_odom_ptr->pose.pose; + planner_data.ego_vel = ego_odom_ptr->twist.twist.linear.x; debug_data_ptr_->ego_pose = planner_data.ego_pose; return planner_data; From a553c6db3dca9fbf7b2227fbd1d09a3dd1da65fb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 2 Jun 2024 10:53:38 +0900 Subject: [PATCH 17/23] feat(path_smoother): use polling subscriber (#7214) Signed-off-by: Takayuki Murooka --- .../path_smoother/elastic_band_smoother.hpp | 9 ++++----- .../path_smoother/src/elastic_band_smoother.cpp | 16 ++++++++-------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index e0bdb326adb33..bc566fdfb96bf 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -22,6 +22,7 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; + bool isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6fb732309da4b..75300286ac9dc 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + const auto ego_state_ptr = odom_sub_.takeData(); + if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } @@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // 1. calculate trajectory with Elastic Band // 1.a check if replan (= optimization) is required PlannerData planner_data( - input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + input_traj_points, ego_state_ptr->pose.pose, ego_state_ptr->twist.twist.linear.x); const bool is_replan_required = [&]() { if (replan_checker_ptr_->isResetRequired(planner_data)) { // NOTE: always replan when resetting previous optimization @@ -195,7 +194,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( - input_traj_points, ego_state_ptr_->pose.pose) + input_traj_points, ego_state_ptr->pose.pose) : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); @@ -203,7 +202,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) std::make_shared>(smoothed_traj_points); // 2. update velocity - applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); @@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } -bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ElasticBandSmoother::isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { + if (!ego_state_ptr) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); return false; } From 85fef81ebb1c836cecd4588f301b7ca1a3607437 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 2 Jun 2024 14:45:17 +0900 Subject: [PATCH 18/23] fix(yabloc): fix bug in capturing in lambda function (#7208) * fix(yabloc): fix bug in capturing in lambda function 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> --- .../yabloc/yabloc_common/src/extract_line_segments.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 9987c4fbb5e72..98c70b44c63fc 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -25,7 +25,8 @@ pcl::PointCloud extract_near_line_segments( // All line segments contained in a square with max_range on one side must be taken out, // so pick up those that are closer than the **diagonals** of the square. - auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { + auto check_intersection = [sqrt_two, max_range, + pose_vector](const pcl::PointNormal & pn) -> bool { const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; From 573d816853fdd5228924ca2934a803c6986db7f4 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 2 Jun 2024 14:45:35 +0900 Subject: [PATCH 19/23] refactor(yabloc): use constexpr properly (#7207) Signed-off-by: Ryuta Kambe --- localization/yabloc/yabloc_common/src/extract_line_segments.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 98c70b44c63fc..9940dca1fd62b 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -20,7 +20,7 @@ pcl::PointCloud extract_near_line_segments( const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, const float max_range) { - constexpr double sqrt_two = std::sqrt(2); + constexpr double sqrt_two = 1.41421356237309504880; const Eigen::Vector3f pose_vector = transform.translation(); // All line segments contained in a square with max_range on one side must be taken out, From 8416d2c26a3099cffea3105239449b023e8db06f Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Sun, 2 Jun 2024 14:47:44 +0900 Subject: [PATCH 20/23] fix(autoware_auto_common): nullptr_t (#7212) Signed-off-by: Shumpei Wakabayashi --- .../helper_functions/message_adapters.hpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index d3bda57b3374f..cc2fb0e41c372 100644 --- a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -36,7 +36,7 @@ using TimeStamp = builtin_interfaces::msg::Time; /// \brief Helper class to check existence of header file in compile time: /// https://stackoverflow.com/a/16000226/2325407 -template +template struct HasHeader : std::false_type { }; @@ -48,60 +48,60 @@ struct HasHeader : std::true_type /////////// Template declarations -/// Get frame id from message. nullptr_t is used to prevent template ambiguity on +/// Get frame id from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const std::string & get_frame_id(const T & msg) noexcept; -/// Get a reference to the frame id from message. nullptr_t is used to prevent +/// Get a reference to the frame id from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template std::string & get_frame_id(T & msg) noexcept; -/// Get stamp from message. nullptr_t is used to prevent template ambiguity on +/// Get stamp from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const TimeStamp & get_stamp(const T & msg) noexcept; -/// Get a reference to the stamp from message. nullptr_t is used to prevent +/// Get a reference to the stamp from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template TimeStamp & get_stamp(T & msg) noexcept; /////////////// Default specializations for message types that contain a header. -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> const std::string & get_frame_id(const T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> std::string & get_frame_id(T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp & get_stamp(T & msg) noexcept { return msg.header.stamp; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp get_stamp(const T & msg) noexcept { return msg.header.stamp; From e132ad75408ac62191d707b961cffea0f7762745 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Mon, 3 Jun 2024 09:42:23 +0900 Subject: [PATCH 21/23] test(image_projection_based_fusion): add unit test code for geometry (#7096) * add geometry utility test code * style(pre-commit): autofix * fix scope and declare * change declare name * style(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 3 + .../test/test_geometry.cpp | 206 ++++++++++++++++++ 2 files changed, 209 insertions(+) create mode 100644 perception/image_projection_based_fusion/test/test_geometry.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 825c4888ac7ea..0f4cb7112f74a 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -168,6 +168,9 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_utils test/test_utils.cpp ) + ament_auto_add_gtest(test_geometry + test/test_geometry.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/test/test_geometry.cpp b/perception/image_projection_based_fusion/test/test_geometry.cpp new file mode 100644 index 0000000000000..ac8160fe89124 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_geometry.cpp @@ -0,0 +1,206 @@ +// 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 "image_projection_based_fusion/utils/geometry.hpp" + +#include + +TEST(objectToVertices, test_objectToVertices) +{ + // Test `boundingBoxToVertices()` and `cylinderToVertices()` simultaneously + // Test at Shape::BOUNDING_BOX + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + pose.position.z = 3.0; + const double angle = M_PI / 12; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = std::sin(angle); + pose.orientation.w = std::cos(angle); + { + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 0; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 1.2320508075688772935274, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 5.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), -1.0, 1e-6); + EXPECT_NEAR(vertices.at(7).x(), -2.232050807568877293527, 1e-6); + EXPECT_NEAR(vertices.at(7).y(), 3.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(7).z(), 7.0, 1e-6); + } + + { + // Test at Shape::CYLINDER + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 1; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 3.0, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), 7.0, 1e-6); + EXPECT_NEAR(vertices.at(11).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(11).y(), 1.0, 1e-6); + EXPECT_NEAR(vertices.at(11).z(), -1.0, 1e-6); + } + + { + // Test at Shape::POLYGON (Nothing to do) + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 2; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_TRUE(vertices.empty()); + } +} + +TEST(transformPoints, test_transformPoints) +{ + std::vector input_points; + Eigen::Vector3d point(0.0, 0.0, 0.0); + input_points.push_back(point); + Eigen::Translation translation(1.0, 1.0, 1.0); + Eigen::Matrix3d rotation = (Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())) + .toRotationMatrix(); + Eigen::Affine3d affine_transform = rotation * translation; + std::vector output_points; + + image_projection_based_fusion::transformPoints(input_points, affine_transform, output_points); + + EXPECT_FALSE(output_points.empty()); + EXPECT_NEAR(output_points.at(0).x(), 0.7071067811865475244008, 1e-6); + EXPECT_NEAR(output_points.at(0).y(), 0.5, 1e-6); + EXPECT_NEAR(output_points.at(0).z(), 1.5, 1e-6); +} + +TEST(is_inside, test_is_inside) +{ + // Test default pattern + sensor_msgs::msg::RegionOfInterest outer; + outer.x_offset = 30; + outer.y_offset = 40; + outer.height = 400; + outer.width = 300; + const double outer_offset_scale = 1.0; + { + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 399; + inner.width = 299; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_TRUE(inside_flag); + } + + { + // Test left-top outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 29; + inner.y_offset = 39; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } + + { + // Test right-bottom outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 401; + inner.width = 301; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } +} + +TEST(sanitizeROI, test_sanitizeROI) +{ + { + // Test default pattern + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 200; + roi.width = 100; + int height = 400; // image height + int width = 300; // image width + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 200); + EXPECT_EQ(roi.width, 100); + } + + { + // Test pattern that x_offset or y_offset is not in image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 100; + roi.y_offset = 200; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 0); + EXPECT_EQ(roi.width, 0); + } + + { + // Test patten that roi does not fit within image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 500; + roi.width = 400; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 80); + EXPECT_EQ(roi.width, 40); + } +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From cb739f17f6960bbb3e3529a29f3c7ce2c8b1d07b Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 3 Jun 2024 10:09:22 +0900 Subject: [PATCH 22/23] feat(detected_object_validation): filter unknown objects with its footprint (#7186) feat: filter unknown objects with its footprint Signed-off-by: yoshiri --- .../object_lanelet_filter.hpp | 3 ++ .../utils/utils.hpp | 18 +++++++- .../src/object_lanelet_filter.cpp | 45 ++++++++++++++----- 3 files changed, 55 insertions(+), 11 deletions(-) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 00826de68157f..6c652d0ad093e 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -80,6 +80,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); + bool isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); bool isSameDirectionWithLanelets( const lanelet::ConstLanelets & lanelets, diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp index 2c46e1b9dd110..853733dc0ee5f 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp @@ -15,8 +15,9 @@ #ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#include +#include +#include namespace utils { struct FilterTargetLabel @@ -31,6 +32,21 @@ struct FilterTargetLabel bool PEDESTRIAN; bool isTarget(const uint8_t label) const; }; // struct FilterTargetLabel + +inline bool hasBoundingBox(const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return true; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return true; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + return false; + } else { + // unknown shape type. + return false; + } +} + } // namespace utils #endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index fe304a3ea22bb..80a2ce4b6563f 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -139,17 +139,9 @@ bool ObjectLaneletFilterNode::filterObject( bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { - Polygon2d polygon; - const auto footprint = setFootprint(transformed_object); - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( - point, transformed_object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); const bool is_polygon_overlap = - isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || - isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); + isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); filter_pass = filter_pass && is_polygon_overlap; } @@ -234,6 +226,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets; } +bool ObjectLaneletFilterNode::isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets) +{ + // if has bounding box, use polygon overlap + if (utils::hasBoundingBox(object)) { + Polygon2d polygon; + const auto footprint = setFootprint(object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + return isPolygonOverlapLanelets(polygon, intersected_lanelets); + } else { + // if object do not have bounding box, check each footprint is inside polygon + for (const auto & point : object.shape.footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + geometry_msgs::msg::Pose point2d; + point2d.position.x = point_transformed.x; + point2d.position.y = point_transformed.y; + for (const auto & lanelet : intersected_lanelets) { + if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + return true; + } + } + } + return false; + } +} + bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) { From 1b7c600e82cacdeaf540f7ff5bce1523c6c2af9a Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 10:38:55 +0900 Subject: [PATCH 23/23] feat: remove autoware_auto_geometry package (#7217) * feat!: remove autoware_auto_geometry package Signed-off-by: Ryohsuke Mitsudome * docs: remove autoware_auto_geometry package from docs Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - common/.pages | 4 - common/autoware_auto_geometry/CMakeLists.txt | 39 -- .../autoware_auto_geometry/design/interval.md | 111 ---- .../design/polygon_intersection_2d-design.md | 51 -- .../design/spatial-hash-design.md | 99 --- .../bounding_box/bounding_box_common.hpp | 188 ------ .../bounding_box/eigenbox_2d.hpp | 247 -------- .../bounding_box/lfit.hpp | 281 --------- .../bounding_box/rotating_calipers.hpp | 280 --------- .../bounding_box_2d.hpp | 34 - .../autoware_auto_geometry/common_2d.hpp | 587 ------------------ .../autoware_auto_geometry/common_3d.hpp | 77 --- .../autoware_auto_geometry/convex_hull.hpp | 195 ------ .../autoware_auto_geometry/hull_pockets.hpp | 111 ---- .../autoware_auto_geometry/intersection.hpp | 312 ---------- .../autoware_auto_geometry/interval.hpp | 358 ----------- .../autoware_auto_geometry/lookup_table.hpp | 179 ------ .../autoware_auto_geometry/spatial_hash.hpp | 332 ---------- .../spatial_hash_config.hpp | 450 -------------- .../visibility_control.hpp | 41 -- common/autoware_auto_geometry/package.xml | 30 - .../src/bounding_box.cpp | 157 ----- .../src/spatial_hash.cpp | 104 ---- .../test/include/test_bounding_box.hpp | 550 ---------------- .../test/include/test_spatial_hash.hpp | 260 -------- .../test/src/lookup_table.cpp | 166 ----- .../test/src/test_area.cpp | 132 ---- .../test/src/test_common_2d.cpp | 197 ------ .../test/src/test_convex_hull.cpp | 372 ----------- .../test/src/test_geometry.cpp | 25 - .../test/src/test_hull_pockets.cpp | 182 ------ .../test/src/test_intersection.cpp | 155 ----- .../test/src/test_interval.cpp | 259 -------- .../package.xml | 1 - 35 files changed, 6567 deletions(-) delete mode 100644 common/autoware_auto_geometry/CMakeLists.txt delete mode 100644 common/autoware_auto_geometry/design/interval.md delete mode 100644 common/autoware_auto_geometry/design/polygon_intersection_2d-design.md delete mode 100644 common/autoware_auto_geometry/design/spatial-hash-design.md delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp delete mode 100644 common/autoware_auto_geometry/package.xml delete mode 100644 common/autoware_auto_geometry/src/bounding_box.cpp delete mode 100644 common/autoware_auto_geometry/src/spatial_hash.cpp delete mode 100644 common/autoware_auto_geometry/test/include/test_bounding_box.hpp delete mode 100644 common/autoware_auto_geometry/test/include/test_spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/test/src/lookup_table.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_area.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_common_2d.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_convex_hull.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_geometry.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_hull_pockets.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_intersection.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_interval.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5f5d5008c0f1c..e60173068f028 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,6 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_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_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai diff --git a/common/.pages b/common/.pages index f1ecfc71f96b6..5ce88e0561c34 100644 --- a/common/.pages +++ b/common/.pages @@ -6,10 +6,6 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons - - 'autoware_auto_geometry': - - 'interval': common/autoware_auto_geometry/design/interval - - 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design - - 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt deleted file mode 100644 index ee819b7cd797c..0000000000000 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_geometry) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - include/autoware_auto_geometry/spatial_hash.hpp - include/autoware_auto_geometry/intersection.hpp - include/autoware_auto_geometry/spatial_hash_config.hpp - src/spatial_hash.cpp - src/bounding_box.cpp -) - -if(BUILD_TESTING) - set(GEOMETRY_GTEST geometry_gtest) - set(GEOMETRY_SRC test/src/test_geometry.cpp - test/src/test_convex_hull.cpp - test/src/test_hull_pockets.cpp - test/src/test_interval.cpp - test/src/lookup_table.cpp - test/src/test_area.cpp - test/src/test_common_2d.cpp - test/src/test_intersection.cpp - ) - ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) - target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) - target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") - ament_target_dependencies(${GEOMETRY_GTEST} - "autoware_auto_common" - "autoware_auto_geometry_msgs" - "autoware_auto_planning_msgs" - "autoware_auto_vehicle_msgs" - "geometry_msgs" - "osrf_testing_tools_cpp") - target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md deleted file mode 100644 index 4fe65ff8e0310..0000000000000 --- a/common/autoware_auto_geometry/design/interval.md +++ /dev/null @@ -1,111 +0,0 @@ -# Interval - -The interval is a standard 1D real-valued interval. -The class implements a representation and operations on the interval type and guarantees interval validity on construction. -Basic operations and accessors are implemented, as well as other common operations. -See 'Example Usage' below. - -## Target use cases - -- Range or containment checks. - The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. - It also provides consistent behavior and consistent handling of edge cases. - -## Properties - -- **empty**: An empty interval is equivalent to an empty set. - It contains no elements. - It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero. - The implementation represents the measure of an empty interval with `NaN`. -- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. - The measure is zero because the interval contains only a single point, and points have zero measure. - However, because it does contain a single element, the interval is _not_ empty. -- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. - An attempt to construct an invalid interval results in a runtime_error exception being thrown. -- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. - -## Conventions - -- All operations on interval objects are defined as static class methods on the interval class. - This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. - -## Assumptions - -- The interval is only intended for floating point types. - This is enforced via static assertion. -- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). - If this assumption is violated, an exception is emitted and construction fails. - -## Example Usage - -```c++ -#include "autoware_auto_geometry/interval.hpp" - -#include - -// using-directive is just for illustration; don't do this in practice -using namespace autoware::common::geometry; - -// bounds for example interval -constexpr auto MIN = 0.0; -constexpr auto MAX = 1.0; - -// -// Try to construct an invalid interval. This will give the following error: -// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' -// - -try { - const auto i = Interval_d(MAX, MIN); -} catch (const std::runtime_error& e) { - std::cerr << e.what(); -} - -// -// Construct a double precision interval from 0 to 1 -// - -const auto i = Interval_d(MIN, MAX); - -// -// Test accessors and properties -// - -std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; -// Prints: 0.0 1.0 - -std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; -// Prints: false 1.0 - -std::cout << Interval_d::contains(i, 0.3) << "\n"; -// Prints: true - -std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; -// Prints: true - -// -// Test operations. -// - -std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; -// Prints: {"min": 0.0, "max": 0.3} - -std::cout << Interval_d::project_to_interval(i, 0.5) << " " - << Interval_d::project_to_interval(i, -1.3) << "\n"; -// Prints: 0.5 0.0 - -// -// Distinguish empty/zero measure -// - -const auto i_empty = Interval(); -const auto i_zero_length = Interval(0.0, 0.0); - -std::cout << Interval_d::empty(i_empty) << " " - << Interval_d::empty(i_zero_length) << "\n"; -// Prints: true false - -std::cout << Interval_d::zero_measure(i_empty) << " " - << Interval_d::zero_measure(i_zero_length) << "\n"; -// Prints: false false -``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md deleted file mode 100644 index f651c218bc80d..0000000000000 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ /dev/null @@ -1,51 +0,0 @@ -# 2D Convex Polygon Intersection - -Two convex polygon's intersection can be visualized on the image below as the blue area: - - - -## Purpose / Use cases - -Computing the intersection between two polygons can be useful in many applications of scene -understanding. It can be used to estimate collision detection, shape alignment, shape -association and in any application that deals with the objects around the perceiving agent. - -## Design - -[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following -observations about convex polygon intersection: - -- Intersection of two convex polygons is a convex polygon -- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection - shape. (Vertices A, C, D in the shape above) -- An edge from a polygon that is contained in the other polygon is an edge in the intersection - shape. (edge C-D in the shape above) -- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, - E in the shape above.) - -### Inner-workings / Algorithms - -With the observation mentioned above, the current algorithm operates in the following way: - -- Compute and find the vertices from each polygon that is contained in the other polygon - (Vertices A, C, D) -- Compute and find the intersection points between each polygon (Vertices B, E) -- Compute the convex hull shaped by these vertices by ordering them CCW. - -### Inputs / Outputs / API - -Inputs: - -- Two iterables that contain vertices of the convex polygons ordered in the CCW direction. - -Outputs: - -- A list of vertices of the intersection shape ordered in the CCW direction. - -## Future Work - -- #1230: Applying efficient algorithms. - -## Related issues - -- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md deleted file mode 100644 index 58eecf3ee841b..0000000000000 --- a/common/autoware_auto_geometry/design/spatial-hash-design.md +++ /dev/null @@ -1,99 +0,0 @@ -# Spatial Hash - -The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in -low dimensions. - -The fixed-radius near-neighbors problem is defined as follows: - -`For point p, find all points p' s.t. d(p, p') < r` - -Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed -radius. - -For `n` points with an average of `k` neighbors each, this data structure can -perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` -different points) in `O(mk)` time. - -By contrast, using a k-d tree for successive nearest-neighbor queries results in -a running time of `O(m log n)`. - -The spatial hash works as follows: - -- Each point is assigned to a bin in the predefined bounding area defined by - `x_min/x_max` and `y_min/y_max` -- This can be done by converting x and y position into x and y index - respectively - - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` - - The two (or more) indices can then be converted into a single index -- Once every point of interest has been inserted into the hash, near-neighbor - queries can begin: - - The bin of the reference point is first computed - - For each point in each adjacent bin, perform an explicit distance computation - between said point and the reference point. If the distance is below the given - radius, said point is considered to be a near-neighbor - -Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. -The bin size was computed to be the same as the lookup distance. - - - -In addition, this data structure can support 2D or 3D queries. This is determined during -configuration, and baked into the data structure via the configuration class. The purpose of -this was to avoid if statements in tight loops. The configuration class specializations themselves -use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid -a dispatching call. - -## Performance characterization - -### Time - -Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for -hashmaps. In practice, lookup time for hashmaps and thus insertion time should -be `O(1)`. - -Removing a point is `O(1)` because the current API only supports removal via -direct reference to a node. - -Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial -example, but in practice `O(k)`. - -### Space - -The module consists of the following components: - -- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary - constant (load factor) -- The other components of the spatial hash are `O(n + n)` - -This results in `O(n)` space complexity. - -## States - -The spatial hash's state is dictated by the status of the underlying unordered_multimap. - -The data structure is wholly configured by a -[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor -of the class determines in the data structure accepts strictly 2D or strictly 3D queries. - -## Inputs - -The primary method of introducing data into the data structure is via the -[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. - -## Outputs - -The primary method of retrieving data from the data structure is via the -[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D -configuration\) -or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) -\(3D configuration\) method. - -The whole data structure can also be traversed using standard constant iterators. - -## Future Work - -- Performance tuning and optimization - -## Related issues - -- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp deleted file mode 100644 index 0f3a68e14d442..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief Common functionality for bounding box computation algorithms - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Functions and types for generating enclosing bounding boxes around a set of points -namespace bounding_box -{ -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin The start of the list of points -/// \param[in] end One past the end of the list of points -/// \param[out] box A box for which the z component of centroid, corners, and size gets filled -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, BoundingBox & box) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - box.centroid.z = (max_z + min_z) * 0.5F; - for (auto & corner : box.corners) { - corner.z = box.centroid.z; - } - box.size.z = (max_z - min_z) * 0.5F; -} - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin Iterator pointing to the start of the range of points -/// \param[in] end Iterator pointing the the end of the range of points -/// \param[out] shape A shape in which vertices z values and height field will be set -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - for (auto & corner : shape.footprint.points) { - corner.z = min_z; - } - shape.dimensions.z = max_z - min_z; -} - -namespace details -{ -/// Templated alias for getting a type without CV or reference qualification -template -using base_type = typename std::remove_cv::type>::type; - -/// Templated alias for an array of 4 objects of type PointT -template -using Point4 = std::array; - -/// \brief Helper struct for compile time introspection of array size from -/// Ref: https://stackoverflow.com/questions/16866033 -template -struct array_size; -template -struct array_size> -{ - static std::size_t const size = N; -}; -static_assert( - array_size>::size == 4U, - "Bounding box does not have the right number of corners"); - -/// \brief Compute length, width, area of bounding box -/// \param[in] corners Corners of the current bounding box -/// \param[out] ret A point struct used to hold size of box defined by corners -void GEOMETRY_PUBLIC -size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); -/// \brief Computes centroid and orientation of a box given corners -/// \param[in] corners Array of final corners of bounding box -/// \param[out] box Message to have corners, orientation, and centroid updated -void GEOMETRY_PUBLIC -finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); - -/// \brief given support points and two orthogonal directions, compute corners of bounding box -/// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferenceable into PointT -/// \param[out] corners Gets filled with corner points of bounding box -/// \param[in] supports Iterators referring to support points of current bounding box -/// e.g. bounding box is touching these points -/// \param[in] directions Directions of each edge of the bounding box from each support point -template -void compute_corners( - decltype(BoundingBox::corners) & corners, const Point4 & supports, - const Point4 & directions) -{ - for (uint32_t idx = 0U; idx < corners.size(); ++idx) { - const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; - const auto pt = - intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); - // TODO(c.ho) handle error - point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); - point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); - } -} -// TODO(c.ho) type trait enum base - -/// \brief Copy vertices of the given box into a Shape type -/// \param box Box to be converted -/// \return Shape type filled with box vertices -autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); - -/// \brief Copy centroid and orientation info of the box into Pose type -/// \param box BoundingBox to be converted -/// \return Pose type filled with centroid and orientation from box -geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); - -/// \brief Fill DetectedObject type with contents from a BoundingBox type -/// \param box BoundingBox to be converted -/// \return Filled DetectedObject type -autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC -make_detected_object(const BoundingBox & box); - -/// \brief Transform corners from object-local coordinates using the given centroid and orientation -/// \param shape_msg Msg containing corners in object-local coordinates -/// \param centroid Centroid of the polygon whose corners are defined in shape_msg -/// \param orientation Orientation of the polygon -/// \return corners transformed such that their centroid and orientation correspond to the -/// given inputs -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); - -} // namespace details -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp deleted file mode 100644 index e0f2e66e87ee5..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore eigenbox, EIGENBOX -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief Simplified 2d covariance matrix -struct Covariance2d -{ - /// \brief Variance in the x direction - float32_t xx; - /// \brief Variance in the y direction - float32_t yy; - /// \brief x-y covariance - float32_t xy; - /// \brief Number of points - std::size_t num_points; -}; // struct Covariance2d - -// cspell: ignore Welford -/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return A 2d covariance matrix for all points in the list -template -Covariance2d covariance_2d(const IT begin, const IT end) -{ - Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; - float32_t ux = 0.0F; - float32_t uy = 0.0F; - float32_t num_points = 0.0F; - using point_adapter::x_; - using point_adapter::y_; - for (auto it = begin; it != end; ++it) { - ++ret.num_points; - num_points = static_cast(ret.num_points); - const auto & pt = *it; - // update mean x - const float32_t dx = x_(pt) - ux; - ux = ux + (dx / num_points); - // update cov - const float32_t dy = y_(pt) - uy; - ret.xy += (x_(pt) - ux) * (dy); - // update mean y - uy = uy + (dy / num_points); - // update M2 - ret.xx += dx * (x_(pt) - ux); - ret.yy += dy * (y_(pt) - uy); - } - // finalize sample (co-)variance - if (ret.num_points > 1U) { - num_points = num_points - 1.0F; - } - ret.xx /= num_points; - ret.yy /= num_points; - ret.xy /= num_points; - - return ret; -} - -/// \brief Compute eigenvectors and eigenvalues -/// \param[in] cov 2d Covariance matrix -/// \param[out] eig_vec1 First eigenvector -/// \param[out] eig_vec2 Second eigenvector -/// \tparam PointT Point type that has at least float members x and y -/// \return A pair of eigenvalues: The first is the larger eigenvalue -/// \throw std::runtime error if you would get degenerate covariance -template -std::pair eig_2d( - const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) -{ - const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // Add a small fudge to alleviate floating point errors - float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); - if (disc < 0.0F) { - throw std::runtime_error( - "pca_2d: negative discriminant! Should never happen for well formed " - "covariance matrix"); - } - disc = sqrtf(disc); - // compute eigenvalues - const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); - // compute eigenvectors - using point_adapter::xr_; - using point_adapter::yr_; - // We compare squared value against floating epsilon to make sure that eigen vectors - // are persistent against further calculations. - // (e.g. taking cross product of two eigen vectors) - if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eig_vec1) = cov.xy; - yr_(eig_vec1) = ret.first - cov.xx; - xr_(eig_vec2) = cov.xy; - yr_(eig_vec2) = ret.second - cov.xx; - } else { - if (cov.xx > cov.yy) { - xr_(eig_vec1) = 1.0F; - yr_(eig_vec1) = 0.0F; - xr_(eig_vec2) = 0.0F; - yr_(eig_vec2) = 1.0F; - } else { - xr_(eig_vec1) = 0.0F; - yr_(eig_vec1) = 1.0F; - xr_(eig_vec2) = 1.0F; - yr_(eig_vec2) = 0.0F; - } - } - return ret; -} - -/// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT type of a point with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] eig1 First principal component of cluster -/// \param[in] eig2 Second principal component of cluster -/// \param[out] support Array to get filled with extreme points in each of the principal -/// components -/// \return whether or not eig2 is ccw wrt eig1 -template -bool8_t compute_supports( - const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4 & support) -{ - const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; - std::array metrics{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max(), std::numeric_limits::max()}}; - for (auto it = begin; it != end; ++it) { - const PointT & pt = *it; - float32_t val = dot_2d(ret ? eig1 : eig2, pt); - if (val > metrics[0U]) { - metrics[0U] = val; - support[0U] = it; - } - if (val < metrics[2U]) { - metrics[2U] = val; - support[2U] = it; - } - val = dot_2d(ret ? eig2 : eig1, pt); - if (val > metrics[1U]) { - metrics[1U] = val; - support[1U] = it; - } - if (val < metrics[3U]) { - metrics[3U] = val; - support[3U] = it; - } - } - return ret; -} - -/// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT Point type of the lists, must have float members x and y -/// \param[in] ax1 First basis direction, assumed to be normal to ax2 -/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 -/// \param[in] supports Array of iterators referring to points that are most extreme in each basis -/// direction. Should be result of compute_supports function -/// \return A bounding box based on the basis axes and the support points -template -BoundingBox compute_bounding_box( - const PointT & ax1, const PointT & ax2, const Point4 & supports) -{ - decltype(BoundingBox::corners) corners; - const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; - compute_corners(corners, supports, directions); - - // build box - BoundingBox bbox; - finalize_box(corners, bbox); - size_2d(corners, bbox.size); - return bbox; -} -} // namespace details - -/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not -/// modify the list. The resulting bounding box is not necessarily minimum in any way -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return An oriented bounding box in x-y. This bounding box has no height information -template -BoundingBox eigenbox_2d(const IT begin, const IT end) -{ - // compute covariance - const details::Covariance2d cov = details::covariance_2d(begin, end); - - // compute eigenvectors - using PointT = details::base_type; - PointT eig1; - PointT eig2; - const auto eig_v = details::eig_2d(cov, eig1, eig2); - - // find extreme points - details::Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); - // build box - if (is_ccw) { - std::swap(eig1, eig2); - } - BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eig_v.first; - - return bbox; -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp deleted file mode 100644 index 07fd6c989cedc..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore LFIT, lfit -// LFIT means "L-Shape Fitting" -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief A representation of the M matrix for the L-fit algorithm -struct LFitWs -{ - /// \brief Number of points in the first partition - std::size_t p; - /// \brief Number of points in the second partition - std::size_t q; - // assume matrix of form: [a b; c d] - /// \brief Sum of x values in first partition - float32_t m12a; - /// \brief Sum of y values in first partition - float32_t m12b; - /// \brief Sum of y values in second partition - float32_t m12c; - /// \brief Negative sum of x values in second partition - float32_t m12d; - // m22 is a symmetric matrix - /// \brief Sum_p x_2 + sum_q y_2 - float32_t m22a; - /// \brief Sum_p x*y - sum_q x*y - float32_t m22b; - /// \brief Sum_p y_2 + sum_x y_2 - float32_t m22d; -}; // struct LFitWs - -/// \brief Initialize M matrix for L-fit algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[out] ws A representation of the M matrix to get initialized -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -template -void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) -{ - ws.p = 1UL; - ws.q = size - 1UL; - // init P terms (first partition) - using point_adapter::x_; - using point_adapter::y_; - const auto & pt = *begin; - const float32_t px = x_(pt); - const float32_t py = y_(pt); - // assume matrix of form: [a b; c d] - ws.m12a = px; - ws.m12b = py; - ws.m12c = 0.0F; - ws.m12d = 0.0F; - // m22 is a symmetric matrix - ws.m22a = px * px; - ws.m22b = px * py; - ws.m22d = py * py; - auto it = begin; - ++it; - for (; it != end; ++it) { - const auto & qt = *it; - const float32_t qx = x_(qt); - const float32_t qy = y_(qt); - ws.m12c += qy; - ws.m12d -= qx; - ws.m22a += qy * qy; - ws.m22b -= qx * qy; - ws.m22d += qx * qx; - } -} - -/// \brief Solves the L fit problem for a given M matrix -/// \tparam PointT The point type of the cluster being L-fitted -/// \param[in] ws A representation of the M Matrix -/// \param[out] dir The best fit direction for this partition/M matrix -/// \return The L2 residual for this fit (the score, lower is better) -template -float32_t solve_lfit(const LFitWs & ws, PointT & dir) -{ - const float32_t pi = 1.0F / static_cast(ws.p); - const float32_t qi = 1.0F / static_cast(ws.q); - const Covariance2d M{// matrix of form [x, z; z y] - ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), - ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), - ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; - PointT eig1; - const auto eig_v = eig_2d(M, eig1, dir); - return eig_v.second; -} - -/// \brief Increments L fit M matrix with information in the point -/// \tparam PointT The point type -/// \param[in] pt The point to increment the M matrix with -/// \param[inout] ws A representation of the M matrix -template -void increment_lfit_ws(const PointT & pt, LFitWs & ws) -{ - const float32_t px = point_adapter::x_(pt); - const float32_t py = point_adapter::y_(pt); - ws.m12a += px; - ws.m12b += py; - ws.m12c -= py; - ws.m12d += px; - ws.m22b += 2.0F * px * py; - const float32_t px2y2 = (px - py) * (px + py); - ws.m22a += px2y2; - ws.m22d -= px2y2; -} - -/// \tparam IT An iterator type that should dereference into a point type with float members x and y -template -class LFitCompare -{ -public: - /// \brief Constructor, initializes normal direction - /// \param[in] hint A 2d vector with the direction that will be used to order the - /// point list - explicit LFitCompare(const PointT & hint) - : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint)) - { - } - - /// \brief Comparator operation, returns true if the projection of a the tangent line - /// is smaller than the projection of b - /// \param[in] p The first point for comparison - /// \param[in] q The second point for comparison - /// \return True if a has a smaller projection than b on the tangent line - bool8_t operator()(const PointT & p, const PointT & q) const - { - using point_adapter::x_; - using point_adapter::y_; - return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); - } - -private: - const float32_t m_nx; - const float32_t m_ny; -}; // class LFitCompare - -/// \brief The main implementation of L-fitting a bounding box to a list of points. -/// Assumes sufficiently valid, large enough, and appropriately ordered point list -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) -{ - // initialize M - LFitWs ws{}; - init_lfit_ws(begin, end, size, ws); - // solve initial problem - details::base_type best_normal; - float32_t min_eig = solve_lfit(ws, best_normal); - // solve subsequent problems - auto it = begin; - ++it; - for (; it != end; ++it) { - // update M - ws.p += 1U; - ws.q -= 1U; - if (ws.q == 0U) { // checks for q = 0 case - break; - } - increment_lfit_ws(*it, ws); - // solve incremented problem - decltype(best_normal) dir; - const float32_t score = solve_lfit(ws, dir); - // update optima - if (score < min_eig) { - min_eig = score; - best_normal = dir; - } - } - // can recover best corner point, but don't care, need to cover all points - const auto inv_norm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inv_norm)) { - throw std::runtime_error{"LFit: Abnormal norm"}; - } - best_normal = times_2d(best_normal, inv_norm); - auto best_tangent = get_normal(best_normal); - // find extreme points - Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); - if (is_ccw) { - std::swap(best_normal, best_tangent); - } - BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); - bbox.value = min_eig; - - return bbox; -} -} // namespace details - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list -/// \return A pair where the first element is an iterator pointing to the nearest point, and the -/// second element is the size of the list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \throw std::domain_error If the number of points is too few -template -BoundingBox lfit_bounding_box_2d( - const IT begin, const IT end, const PointT hint, const std::size_t size) -{ - if (size <= 1U) { - throw std::domain_error("LFit requires >= 2 points!"); - } - // NOTE: assumes points are in base_link/sensor frame! - // sort along tangent wrt sensor origin - // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified - std::partial_sort(begin, end, end, details::LFitCompare{hint}); - - return details::lfit_bounding_box_2d_impl(begin, end, size); -} - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". -/// This implementation sorts the list using std::sort -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) -{ - // use the principal component as the sorting direction - const auto cov = details::covariance_2d(begin, end); - using PointT = details::base_type; - PointT eig1; - PointT eig2; - (void)details::eig_2d(cov, eig1, eig2); - (void)eig2; - return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp deleted file mode 100644 index fb75384eb07cb..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions -/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). -/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] -/// \param[inout] directions Array of directions of current bounding box (in ccw order) -/// \tparam PointT Point type of the lists, must have float members x and y -/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir -template -uint32_t update_angles(const Point4 & edges, Point4 & directions) -{ - // find smallest angle to next - float32_t best_cos_th = -std::numeric_limits::max(); - float32_t best_edge_dir_mag = 0.0F; - uint32_t advance_idx = 0U; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - const float32_t edge_dir_mag = std::max( - norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits::epsilon()); - const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; - if (cos_th > best_cos_th) { - best_cos_th = cos_th; - best_edge_dir_mag = edge_dir_mag; - advance_idx = idx; - } - } - - // update directions by smallest angle - const float32_t sin_th = - cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - rotate_2d(directions[idx], best_cos_th, sin_th); - } - - return advance_idx; -} - -/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] -/// \tparam PointT Point type of the lists, must have float members x and y -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] support Array of points that are most extreme in 4 directions for current OBB -/// \param[out] edges An array to be filled with the polygon edges next from support points -template -void init_edges(const IT begin, const IT end, const Point4 & support, Point4 & edges) -{ - for (uint32_t idx = 0U; idx < support.size(); ++idx) { - auto tmp_it = support[idx]; - ++tmp_it; - const PointT & q = (tmp_it == end) ? *begin : *tmp_it; - edges[idx] = minus_2d(q, *support[idx]); - } -} - -/// \brief Scan through list to find support points for bounding box oriented in direction of -/// u = P[1] - P[0] -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[out] support Array that gets filled with pointers to points that are most extreme in -/// each direction aligned with and orthogonal to the first polygon edge. -/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) -template -void init_bbox(const IT begin, const IT end, Point4 & support) -{ - // compute initial orientation using first two points - auto pt_it = begin; - ++pt_it; - const auto u = minus_2d(*pt_it, *begin); - support[0U] = begin; - std::array metric{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max()}}; - // track u * p, fabsf(u x p), and -u * p - for (pt_it = begin; pt_it != end; ++pt_it) { - // check points against orientation for run_ptr - const auto & pt = *pt_it; - // u * p - const float32_t up = dot_2d(u, pt); - if (up > metric[0U]) { - metric[0U] = up; - support[1U] = pt_it; - } - // -u * p - if (up < metric[2U]) { - metric[2U] = up; - support[3U] = pt_it; - } - // u x p - const float32_t uxp = cross_2d(u, pt); - if (uxp > metric[1U]) { - metric[1U] = uxp; - support[2U] = pt_it; - } - } -} -/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. -/// This function may possibly also be used for computing the width of a convex hull, as it uses the -/// external supports of a single convex hull. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect -/// to -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) -/// of a bounding box, assumed to be packed in a Point32 message. -/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding -/// box -template -BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) -{ - using PointT = base_type; - // sanity check TODO(c.ho) more checks (up to n = 2?) - if (begin == end) { - throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); - } - // initial scan to get anchor points - Point4 support; - init_bbox(begin, end, support); - // initialize directions accordingly - Point4 directions; - { // I just don't want the temporary variable floating around - auto tmp = support[0U]; - ++tmp; - directions[0U] = minus_2d(*tmp, *support[0U]); - directions[1U] = get_normal(directions[0U]); - directions[2U] = minus_2d(directions[0U]); - directions[3U] = minus_2d(directions[1U]); - } - // initialize edges - Point4 edges; - init_edges(begin, end, support, edges); - // size of initial guess - BoundingBox bbox; - decltype(BoundingBox::corners) best_corners; - compute_corners(best_corners, support, directions); - size_2d(best_corners, bbox.size); - bbox.value = metric_fn(bbox.size); - // rotating calipers step: incrementally advance, update angles, points, compute area - for (auto it = begin; it != end; ++it) { - // find smallest angle to next, update directions - const uint32_t advance_idx = update_angles(edges, directions); - // recompute area - decltype(BoundingBox::corners) corners; - compute_corners(corners, support, directions); - decltype(BoundingBox::size) tmp_size; - size_2d(corners, tmp_size); - const float32_t tmp_value = metric_fn(tmp_size); - // update best if necessary - if (tmp_value < bbox.value) { - // corners: memcpy is fine since I know corners and best_corners are distinct - (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); - // size - bbox.size = tmp_size; - bbox.value = tmp_value; - } - // Step to next iteration of calipers - { - // update smallest support - auto next_it = support[advance_idx]; - ++next_it; - const auto run_it = (end == next_it) ? begin : next_it; - support[advance_idx] = run_it; - // update edges - next_it = run_it; - ++next_it; - const PointT & next = (end == next_it) ? (*begin) : (*next_it); - edges[advance_idx] = minus_2d(next, *run_it); - } - } - - finalize_box(best_corners, bbox); - - // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 - // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, - // it would probably get smoothed out by prediction. But, this could cause issues with the - // collision detection algorithms (i.e GJK), but probably not. - - return bbox; -} -} // namespace details - -/// \brief Compute the minimum area bounding box given a convex hull of points. -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_area_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x * pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum perimeter bounding box given a convex hull of points -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x + pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum area bounding box given an unstructured list of points. -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum area bounding box, value field is the area -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_area_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_area_bounding_box(list.cbegin(), last); -} - -/// \brief Compute the minimum perimeter bounding box given an unstructured list of points -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_perimeter_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_perimeter_bounding_box(list.cbegin(), last); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp deleted file mode 100644 index c4c52928ac19a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp deleted file mode 100644 index e3c2e58c9f80e..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ /dev/null @@ -1,587 +0,0 @@ -// Copyright 2017-2021 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 2D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace comparison = autoware::common::helper_functions::comparisons; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types -namespace point_adapter -{ -/// \brief Gets the x value for a point -/// \return The x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto x_(const PointT & pt) -{ - return pt.x; -} -/// \brief Gets the x value for a TrajectoryPoint message -/// \return The x value of the point -/// \param[in] pt The point -inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets the y value for a point -/// \return The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto y_(const PointT & pt) -{ - return pt.y; -} -/// \brief Gets the y value for a TrajectoryPoint message -/// \return The y value of the point -/// \param[in] pt The point -inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets the z value for a point -/// \return The z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto z_(const PointT & pt) -{ - return pt.z; -} -/// \brief Gets the z value for a TrajectoryPoint message -/// \return The z value of the point -/// \param[in] pt The point -inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -/// \brief Gets a reference to the x value for a point -/// \return A reference to the x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & xr_(PointT & pt) -{ - return pt.x; -} -/// \brief Gets a reference to the x value for a TrajectoryPoint -/// \return A reference to the x value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets a reference to the y value for a point -/// \return A reference to The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & yr_(PointT & pt) -{ - return pt.y; -} -/// \brief Gets a reference to the y value for a TrajectoryPoint -/// \return A reference to the y value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets a reference to the z value for a point -/// \return A reference to the z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & zr_(PointT & pt) -{ - return pt.z; -} -/// \brief Gets a reference to the z value for a TrajectoryPoint -/// \return A reference to the z value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -} // namespace point_adapter - -namespace details -{ -// Return the next iterator, cycling back to the beginning of the list if you hit the end -template -IT circular_next(const IT begin, const IT end, const IT current) noexcept -{ - auto next = std::next(current); - if (end == next) { - next = begin; - } - return next; -} - -} // namespace details - -/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y -/// \brief compute whether line segment rp is counter clockwise relative to line segment qp -/// \param[in] pt shared point for both line segments -/// \param[in] r point to check if it forms a ccw angle -/// \param[in] q reference point -/// \return whether angle formed is ccw. Three collinear points is considered ccw -template -inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) -{ - using point_adapter::x_; - using point_adapter::y_; - return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p x q = p1 * q2 - p2 * q1 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d cross product -template -inline auto cross_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p * q = p1 * q1 + p2 * q2 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d scalar dot product -template -inline auto dot_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the 2d difference between two points, p - q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the difference in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) - x_(q); - point_adapter::yr_(r) = y_(p) - y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The unary minus or negation operator applied to a single point's 2d fields -/// \param[in] p The left hand side -/// \return A point with the negation in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p) -{ - T r; - point_adapter::xr_(r) = -point_adapter::x_(p); - point_adapter::yr_(r) = -point_adapter::y_(p); - return r; -} -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The 2d addition operation, p + q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the sum in the x and y fields, all other fields are default -/// initialized -template -T plus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) + x_(q); - point_adapter::yr_(r) = y_(p) + y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The scalar multiplication operation, p * a -/// \param[in] p The point value -/// \param[in] a The scalar value -/// \return A point with the scaled x and y fields, all other fields are default -/// initialized -template -T times_2d(const T & p, const float32_t a) -{ - T r; - point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; - point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief solve p + t * u = q + s * v -/// Ref: https://stackoverflow.com/questions/563198/ -/// \param[in] pt anchor point of first line -/// \param[in] u direction of first line -/// \param[in] q anchor point of second line -/// \param[in] v direction of second line -/// \return intersection point -/// \throw std::runtime_error if lines are (nearly) collinear or parallel -template -inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) -{ - const float32_t num = cross_2d(minus_2d(pt, q), u); - float32_t den = cross_2d(v, u); - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - if (fabsf(den) < FEPS) { - if (fabsf(num) < FEPS) { - // collinear case, anything is ok - den = 1.0F; - } else { - // parallel case, no valid output - throw std::runtime_error( - "intersection_2d: no unique solution (either collinear or parallel)"); - } - } - return plus_2d(q, times_2d(v, num / den)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate point given precomputed sin and cos -/// \param[inout] pt point to rotate -/// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precomputed sine value -template -inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) -{ - const float32_t x = point_adapter::x_(pt); - const float32_t y = point_adapter::y_(pt); - point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); - point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate by radian angle th in z direction with ccw positive -/// \param[in] pt reference point to rotate -/// \param[in] th_rad angle by which to rotate point -/// \return rotated point -template -inline T rotate_2d(const T & pt, const float32_t th_rad) -{ - T q(pt); // It's reasonable to expect a copy constructor - const float32_t s = sinf(th_rad); - const float32_t c = cosf(th_rad); - rotate_2d(q, c, s); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief compute q s.t. p T q, or p * q = 0 -/// This is the equivalent of a 90 degree ccw rotation -/// \param[in] pt point to get normal point of -/// \return point normal to p (un-normalized) -template -inline T get_normal(const T & pt) -{ - T q(pt); - point_adapter::xr_(q) = -point_adapter::y_(pt); - point_adapter::yr_(q) = point_adapter::x_(pt); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief get magnitude of x and y components: -/// \param[in] pt point to get magnitude of -/// \return magnitude of x and y components together -template -inline auto norm_2d(const T & pt) -{ - return sqrtf(static_cast(dot_2d(pt, pt))); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on line segment p-q to point r -/// Based on equations from https://stackoverflow.com/a/1501725 and -/// http://paulbourke.net/geometry/pointlineplane/ -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line segment p-q to point r -template -inline T closest_segment_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = static_cast(dot_2d(qp, qp)); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const Interval_f unit_interval(0.0f, 1.0f); - const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; - const float32_t t = Interval_f::clamp_to(unit_interval, val); - ret = plus_2d(p, times_2d(qp, t)); - } - return ret; -} -// -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on the line going through p-q to point r -// Obtained by simplifying closest_segment_point_2d. -/// \param[in] p First point defining the line -/// \param[in] q Second point defining the line -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line p-q to point r -/// \throw std::runtime_error if the two points coincide and hence don't uniquely -// define a line -template -inline T closest_line_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = dot_2d(qp, qp); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; - ret = plus_2d(p, times_2d(qp, t)); - } else { - throw std::runtime_error( - "closet_line_point_2d: line ill-defined because given points coincide"); - } - return ret; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the distance from line segment p-q to point r -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the distance from the line segment to -/// \return Distance from point r to line segment p-q -template -inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) -{ - const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); - return norm_2d(pq_r); -} - -/// \brief Make a 2D unit vector given an angle. -/// \tparam T Point type. Must have point adapters defined or have float members x and y -/// \param th Angle in radians -/// \return Unit vector in the direction of the given angle. -template -inline T make_unit_vector2d(float th) -{ - T r; - point_adapter::xr_(r) = std::cos(th); - point_adapter::yr_(r) = std::sin(th); - return r; -} - -/// \brief Compute squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return squared euclidean distance -template -inline OUT squared_distance_2d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - return (x * x) + (y * y); -} - -/// \brief Compute euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return euclidean distance -template -inline OUT distance_2d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_2d(a, b)); -} - -/// \brief Check the given point's position relative the infinite line passing -/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() -/// \tparam T T point type. Must have point adapters defined or have float members x and y -/// \param p1 point 1 laying on the infinite line -/// \param p2 point 2 laying on the infinite line -/// \param q point to be checked against the line -/// \return > 0 for point q left of the line through p1 to p2 -/// = 0 for point q on the line through p1 to p2 -/// < 0 for point q right of the line through p1 to p2 -template -inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) -{ - return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); -} - -/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise -/// direction): This function does not check for convexity -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Beginning of point sequence -/// \param[in] end One past the last of the point sequence -/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. -/// Returns true for collinear points as well -template -bool all_ordered(const IT begin, const IT end) noexcept -{ - // Short circuit: a line or point is always CCW or otherwise ill-defined - if (std::distance(begin, end) <= 2U) { - return true; - } - bool is_first_point_cw = false; - // Can't use std::all_of because that works directly on the values - for (auto line_start = begin; line_start != end; ++line_start) { - const auto line_end = details::circular_next(begin, end, line_start); - const auto query_point = details::circular_next(begin, end, line_end); - // Check if 3 points starting from current point are in clockwise direction - const bool is_cw = comparison::abs_lte( - check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F); - if (is_cw) { - if (line_start == begin) { - is_first_point_cw = true; - } else { - if (!is_first_point_cw) { - return false; - } - } - } else if (is_first_point_cw) { - return false; - } - } - return true; -} - -/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_2d(const IT begin, const IT end) noexcept -{ - using point_adapter::x_; - using point_adapter::y_; - using T = decltype(x_(*begin)); - auto area = T{}; // zero initialization - // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm - for (auto it = begin; it != end; ++it) { - const auto next = details::circular_next(begin, end, it); - area += x_(*it) * y_(*next); - area -= x_(*next) * y_(*it); - } - return std::abs(T{0.5} * area); -} - -/// Compute area of convex hull, throw if points are not ordered (convexity check is not -/// implemented) -/// \throw std::domain_error if points are not ordered either CW or CCW -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_checked_2d(const IT begin, const IT end) -{ - if (!all_ordered(begin, end)) { - throw std::domain_error{"Cannot compute area: points are not ordered"}; - } - return area_2d(begin, end); -} - -/// \brief Check if the given point is inside or on the edge of the given polygon -/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters -/// defined or have float members x and y -/// \tparam PointType point type. Must have point adapters defined or have float members x and y -/// \param start_it iterator pointing to the first vertex of the polygon -/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in -/// order. -/// \param p point to be searched -/// \return True if the point is inside or on the edge of the polygon. False otherwise -template -bool is_point_inside_polygon_2d( - const IteratorType & start_it, const IteratorType & end_it, const PointType & p) -{ - std::int32_t winding_num = 0; - - for (IteratorType it = start_it; it != end_it; ++it) { - auto next_it = std::next(it); - if (next_it == end_it) { - next_it = start_it; - } - if (point_adapter::y_(*it) <= point_adapter::y_(p)) { - // Upward crossing edge - if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { - if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - ++winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } else { - // Downward crossing edge - if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { - if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - --winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } - } - return winding_num != 0; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp deleted file mode 100644 index 74cd210dec586..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 3D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z -/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 3d scalar dot product -template -inline auto dot_3d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - using point_adapter::z_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); -} - -/// \brief Compute 3D squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return squared 3D euclidean distance -template -inline OUT squared_distance_3d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); - return (x * x) + (y * y) + (z * z); -} - -/// \brief Compute 3D euclidean distance between two points -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return 3D euclidean distance -template -inline OUT distance_3d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_3d(a, b)); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp deleted file mode 100644 index ae81c55ad6b55..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked -/// lists of points - -#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -// lint -e537 NOLINT pclint vs cpplint -#include -// lint -e537 NOLINT pclint vs cpplint -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Contains computation geometry functions not intended for the end user to directly use -namespace details -{ - -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order -/// \param[inout] hull An empty list of points, assumed to have same allocator as points -/// (for splice) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_lower_hull(std::list & points, std::list & hull) -{ - auto hull_it = hull.cbegin(); - auto point_it = points.cbegin(); - // This ensures that the points we splice to back to the end of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to tail of list until point from head of list satisfies ccw - bool8_t is_ccw = true; - while ((hull.cbegin() != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - // return this node to list for consideration in upper hull - points.splice(points.end(), hull, current_hull_it); - } - const auto last_point_it = point_it; - ++point_it; - // Splice head of list to tail of (lower) hull - hull.splice(hull.end(), points, last_point_it); - // point_it has been advanced, hull_it has been advanced (to where point_it was previously) - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain -/// the leftmost point -/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), -/// and to contain the lower hull (minus the left-most point) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_upper_hull(std::list & points, std::list & hull) -{ - // TODO(c.ho) consider reverse iterators, not sure if they work with splice() - auto hull_it = hull.cend(); - --hull_it; - const auto lower_hull_end = hull_it; - auto point_it = points.cend(); - --point_it; - // This ensures that the points we splice to back to the head of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to head of list until ccw is satisfied with tail of list - bool8_t is_ccw = true; - while ((lower_hull_end != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - points.splice(points.begin(), hull, current_hull_it); - } - const auto last_point_it = point_it; - --point_it; - // Splice points from tail of lexically ordered point list to tail of hull - hull.splice(hull.end(), points, last_point_it); - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z direction) -/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull_impl(std::list & list) -{ - // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied - const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { - using point_adapter::x_; - using point_adapter::y_; - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); - }; - list.sort(lexical_comparator); - - // Temporary list to store points - std::list tmp_hull_list{list.get_allocator()}; - - // Shuffle lower hull points over to tmp_hull_list - form_lower_hull(list, tmp_hull_list); - - // Resort list since we can't guarantee the order TODO(c.ho) is this true? - list.sort(lexical_comparator); - // splice first hull point to beginning of list to ensure upper hull is properly closed - // This is done after sorting because we know exactly where it should go, and keeping it out of - // sorting reduces complexity somewhat - list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); - - // build upper hull - form_upper_hull(list, tmp_hull_list); - // Move hull to beginning of list, return iterator pointing to one after the convex hull - const auto ret = list.begin(); - // first move left-most point to ensure it is first - auto tmp_it = tmp_hull_list.end(); - --tmp_it; - list.splice(list.begin(), tmp_hull_list, tmp_it); - // Then move remainder of hull points - list.splice(ret, tmp_hull_list); - return ret; -} -} // namespace details - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z -/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result -/// as previously stated does not hold). -/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull(std::list & list) -{ - return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp deleted file mode 100644 index cd9fd49f71635..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements an algorithm for getting a list of "pockets" in the convex -/// hull of a non-convex simple polygon. - -#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Compute a list of "pockets" of a simple polygon -/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make -/// up the difference between the polygon and its convex hull. -/// This currently has a bug: -// * "Rollover" is not properly handled - if a pocket contains the segment from -// the last point in the list to the first point (which is still part of the -// polygon), that does not get added -/// -/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) -/// \param[in] polygon_end End iterator for the simple polygon -/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon -/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon -/// \return A vector of vectors of the iterator value type. Each inner vector contains the points -/// for one pocket. We return by value instead of as iterator pairs, because it is possible -/// that the edge connecting the final point in the list and the first point in the list is -/// part of a pocket as well, and this is awkward to represent using iterators into the -/// original collection. -/// -/// \tparam Iter1 Iterator to a point type -/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) -template -typename std::vector::value_type>> hull_pockets( - const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, - const Iter2 convex_hull_end) -{ - auto pockets = std::vector::value_type>>{}; - if (std::distance(polygon_start, polygon_end) <= 3) { - return pockets; - } - - // Function to check whether a point is in the convex hull. - const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { - return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) { - return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits::epsilon(); - }); - }; - - // We go through the points of the polygon only once, adding pockets to the list of pockets - // as we detect them. - std::vector::value_type> current_pocket{}; - for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { - if (in_convex_hull(it)) { - if (current_pocket.size() > 1) { - current_pocket.emplace_back(*it); - pockets.push_back(current_pocket); - } - current_pocket.clear(); - current_pocket.emplace_back(*it); - } else { - current_pocket.emplace_back(*it); - } - } - - // At this point, we have reached the end of the polygon, but have not considered the connection - // of the final point back to the first. In case the first point is part of a pocket as well, we - // have some points left in current_pocket, and we add one final pocket that is made up by those - // points plus the first point in the collection. - if (current_pocket.size() > 1) { - current_pocket.push_back(*polygon_start); - pockets.push_back(current_pocket); - } - - return pockets; -} -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp deleted file mode 100644 index 08c70c3a5a6be..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -using autoware::common::geometry::closest_line_point_2d; -using autoware::common::geometry::convex_hull; -using autoware::common::geometry::dot_2d; -using autoware::common::geometry::get_normal; -using autoware::common::geometry::minus_2d; -using autoware::common::geometry::norm_2d; -using autoware::common::geometry::times_2d; -using autoware_auto_perception_msgs::msg::BoundingBox; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -using Point = geometry_msgs::msg::Point32; - -namespace details -{ - -/// Alias for a std::pair of two points -using Line = std::pair; - -/// \tparam Iter1 Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Compute a sorted list of faces of a polyhedron given a list of points -/// \param[in] start Start iterator of the list of points -/// \param[in] end End iterator of the list of points -/// \return The list of faces -template -std::vector get_sorted_face_list(const Iter start, const Iter end) -{ - // First get a sorted list of points - convex_hull does that by modifying its argument - auto corner_list = std::list(start, end); - const auto first_interior_point = convex_hull(corner_list); - - std::vector face_list{}; - auto itLast = corner_list.begin(); - auto itNext = std::next(itLast, 1); - do { - face_list.emplace_back(Line{*itLast, *itNext}); - itLast = itNext; - itNext = std::next(itNext, 1); - } while ((itNext != first_interior_point) && (itNext != corner_list.end())); - - face_list.emplace_back(Line{*itLast, corner_list.front()}); - - return face_list; -} - -/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_contained_points( - const Iterable1T & external, const Iterable2T & internal, - std::list & result) -{ - std::copy_if( - internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) { - return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); - }); -} - -/// \brief Append the intersecting points between two polygons into the output list. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_intersection_points( - const Iterable1T & polygon1, const Iterable2T & polygon2, - std::list & result) -{ - using FloatT = decltype(point_adapter::x_(std::declval())); - using Interval = common::geometry::Interval; - - auto get_edge = [](const auto & list, const auto & iterator) { - const auto next_it = std::next(iterator); - const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); - return std::make_pair(*iterator, next_pt); - }; - - // Get the max absolute value out of two intervals and a scalar. - auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { - auto get_abs_max = [](const auto & interval) { - return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); - }; - return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); - }; - - // Compare each edge from polygon1 to each edge from polygon2 - for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { - const auto & edge1 = get_edge(polygon1, corner1_it); - - Interval edge1_x_interval{ - std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), - std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; - - Interval edge1_y_interval{ - std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), - std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; - - for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { - try { - const auto & edge2 = get_edge(polygon2, corner2_it); - const auto & intersection = common::geometry::intersection_2d( - edge1.first, minus_2d(edge1.second, edge1.first), edge2.first, - minus_2d(edge2.second, edge2.first)); - - Interval edge2_x_interval{ - std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), - std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; - - Interval edge2_y_interval{ - std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), - std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; - - // cspell: ignore feps - // feps means "Float EPSilon" - - // The accumulated floating point error depends on the magnitudes of each end of the - // intervals. Hence the upper bound of the absolute magnitude should be taken into account - // while computing the epsilon. - const auto max_feps_scale = std::max( - compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), - compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))); - const auto feps = max_feps_scale * std::numeric_limits::epsilon(); - // Only accept intersections that lie on both of the line segments (edges) - if ( - Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && - Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) { - result.push_back(intersection); - } - } catch (const std::runtime_error &) { - // Parallel lines. TODO(yunus.caliskan): #1229 - continue; - } - } - } -} - -} // namespace details - -// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion -/// \tparam Iter Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Check if polyhedra defined by two given sets of points intersect -// This uses SAT for doing the actual checking -// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) -/// \param[in] begin1 Start iterator to first list of point types -/// \param[in] end1 End iterator to first list of point types -/// \param[in] begin2 Start iterator to first list of point types -/// \param[in] end2 End iterator to first list of point types -/// \return true if the boxes collide, false otherwise. -template -bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) -{ - // Obtain sorted lists of faces of both boxes, merge them into one big list of faces - auto faces = details::get_sorted_face_list(begin1, end1); - const auto faces_2 = details::get_sorted_face_list(begin2, end2); - faces.reserve(faces.size() + faces_2.size()); - faces.insert(faces.end(), faces_2.begin(), faces_2.end()); - - // Also look at last line - for (const auto & face : faces) { - // Compute normal vector to the face and define a closure to get progress along it - const auto normal = get_normal(minus_2d(face.second, face.first)); - auto get_position_along_line = [&normal](auto point) { - return dot_2d(normal, minus_2d(point, Point{})); - }; - - // Define a function to get the minimum and maximum projected position of the corners - // of a given bounding box along the normal line of the face - auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) { - const auto zero_point = Point{}; - auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); - auto max_corners = min_corners; - - for (auto & point = begin; point != end; ++point) { - const auto point_projected = closest_line_point_2d(normal, zero_point, *point); - const auto position_along_line = get_position_along_line(point_projected); - min_corners = std::min(min_corners, position_along_line); - max_corners = std::max(max_corners, position_along_line); - } - return std::pair{min_corners, max_corners}; - }; - - // Perform the actual computations for the extent computation - auto minmax_1 = get_projected_min_max(begin1, end1); - auto minmax_2 = get_projected_min_max(begin2, end2); - - // Check for any intersections - const auto eps = std::numeric_limits::epsilon(); - if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { - // Found separating hyperplane, stop - return false; - } - } - - // No separating hyperplane found, boxes collide - return true; -} - -/// \brief Get the intersection between two polygons. The polygons should be provided in an -/// identical format to the output of `convex_hull` function as in the corners should be ordered -/// in a CCW fashion. -/// The computation is done by: -/// * Find the corners of each polygon that are contained by the other polygon. -/// * Find the intersection points between two polygons -/// * Combine these points and order CCW to get the final polygon. -/// The criteria for intersection is better explained in: -/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) -/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B -/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient -/// algorithm: #1230 -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam PointT Point type that have the adapters for the x and y fields. -/// set to `Point1T` -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return The resulting conv -template < - template class Iterable1T, template class Iterable2T, typename PointT> -std::list convex_polygon_intersection2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - std::list result; - details::append_contained_points(polygon1, polygon2, result); - details::append_contained_points(polygon2, polygon1, result); - details::append_intersection_points(polygon1, polygon2, result); - const auto end_it = common::geometry::convex_hull(result); - result.resize(static_cast(std::distance(result.cbegin(), end_it))); - return result; -} - -/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons -/// span a zero area, the result is 0.0. -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam Point1T Point type that have the adapters for the x and y fields. -/// \tparam Point2T Point type that have the adapters for the x and y fields. -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the underlying geometrical -/// computation. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -common::types::float32_t convex_intersection_over_union_2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - constexpr auto eps = std::numeric_limits::epsilon(); - const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); - - const auto intersection_area = - common::geometry::area_2d(intersection.begin(), intersection.end()); - - if (intersection_area < eps) { - return 0.0F; // There's either no intersection or the points are collinear - } - - const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end()); - const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end()); - - const auto union_area = polygon1_area + polygon2_area - intersection_area; - if (union_area < eps) { - throw std::domain_error("IoU is undefined for polygons with a zero union area"); - } - - return intersection_area / union_area; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp deleted file mode 100644 index 54be2c32b1d05..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/** - * @brief Data structure to contain scalar interval bounds. - * - * @post The interval is guaranteed to be valid upon successful construction. An - * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds - * are ordered (min <= max). - */ -template -class Interval -{ - static_assert(std::is_floating_point::value, "Intervals only support floating point types."); - -public: - /** - * @brief Compute equality. - * - * Two intervals compare equal iff they are both valid and they are both - * either empty or have equal bounds. - * - * @note This is defined inline because the class is templated. - * - * @return True iff the intervals compare equal. - */ - friend bool operator==(const Interval & i1, const Interval & i2) - { - const auto min_eq = (Interval::min(i1) == Interval::min(i2)); - const auto max_eq = (Interval::max(i1) == Interval::max(i2)); - const auto bounds_equal = (min_eq && max_eq); - const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); - return both_empty || bounds_equal; - } - - /** - * @brief Compute inequality and the logical negation of equality. - * @note This is defined inline because the class is templated. - */ - friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); } - - /** - * @brief Stream overload for Interval types. - * - * @note Output precision is fixed inside the function definition, and the - * serialization is JSON compatible. - * - * @note The serialization is lossy. It is used for debugging and for - * generating exception strings. - */ - friend std::ostream & operator<<(std::ostream & os, const Interval & i) - { - constexpr auto PRECISION = 5; - - // Internal helper to format the output. - const auto readable_extremum = [](const T & val) { - if (val == std::numeric_limits::lowest()) { - return std::string("REAL_MIN"); - } - if (val == std::numeric_limits::max()) { - return std::string("REAL_MAX"); - } - - std::stringstream ss; - ss.setf(std::ios::fixed, std::ios::floatfield); - ss.precision(PRECISION); - ss << val; - return ss.str(); - }; - - // Do stream output. - if (Interval::empty(i)) { - return os << "{}"; - } - return os << "{\"min\": " << readable_extremum(Interval::min(i)) - << ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; - } - - /** - * @brief Test whether the two intervals have bounds within epsilon of each - * other. - * - * @note If both intervals are empty, this returns true. If only one is empty, - * this returns false. - */ - static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); - - /** @brief The minimum bound of the interval. */ - static T min(const Interval & i) { return i.min_; } - - /** @brief The maximum bound of the interval. */ - static T max(const Interval & i) { return i.max_; } - - /** - * @brief Return the measure (length) of the interval. - * - * @note For empty or invalid intervals, NaN is returned. See Interval::empty - * for note on distinction between measure zero and the emptiness property. - */ - static T measure(const Interval & i); - - /** - * @brief Utility for checking whether an interval has zero measure. - * - * @note For empty or invalid intervals, false is returned. See - * Interval::empty for note on distinction between measure zero and the - * emptiness property. - * - * @return True iff the interval has zero measure. - */ - static bool zero_measure(const Interval & i); - - /** - * @brief Whether the interval is empty or not. - * - * @note Emptiness refers to whether the interval contains any points and is - * thus a distinct property from measure: an interval is non-empty if contains - * only a single point even though its measure in that case is zero. - * - * @return True iff the interval is empty. - */ - static bool empty(const Interval & i); - - /** - * @brief Test for whether a given interval contains a given value within the given epsilon - * @return True iff 'interval' contains 'value'. - */ - static bool contains( - const Interval & i, const T & value, const T & eps = std::numeric_limits::epsilon()); - - /** - * @brief Test for whether 'i1' subseteq 'i2' - * @return True iff i1 subseteq i2. - */ - static bool is_subset_eq(const Interval & i1, const Interval & i2); - - /** - * @brief Compute the intersection of two intervals as a new interval. - */ - static Interval intersect(const Interval & i1, const Interval & i2); - - /** - * @brief Clamp a scalar 'val' onto 'interval'. - * @return If 'val' in 'interval', return 'val'; otherwise return the nearer - * interval bound. - */ - static T clamp_to(const Interval & i, T val); - - /** - * @brief Constructor: initialize an empty interval with members set to NaN. - */ - Interval(); - - /** - * @brief Constructor: specify exact interval bounds. - * - * @note An empty interval is specified by setting both bounds to NaN. - * @note An exception is thrown if the specified bounds are invalid. - * - * @post Construction is successful iff the interval is valid. - */ - Interval(const T & min, const T & max); - -private: - static constexpr T NaN = std::numeric_limits::quiet_NaN(); - - T min_; - T max_; - - /** - * @brief Verify that the bounds are valid in an interval. - * @note This method is private because it can only be used in the - * constructor. Once an interval has been constructed, its bounds are - * guaranteed to be valid. - */ - static bool bounds_valid(const Interval & i); -}; // class Interval - -//------------------------------------------------------------------------------ - -typedef Interval Interval_d; -typedef Interval Interval_f; - -//------------------------------------------------------------------------------ - -template -constexpr T Interval::NaN; - -//------------------------------------------------------------------------------ - -template -Interval::Interval() : min_(Interval::NaN), max_(Interval::NaN) -{ -} - -//------------------------------------------------------------------------------ - -template -Interval::Interval(const T & min, const T & max) : min_(min), max_(max) -{ - if (!Interval::bounds_valid(*this)) { - std::stringstream ss; - ss << "Attempted to construct an invalid interval: " << *this; - throw std::runtime_error(ss.str()); - } -} - -//------------------------------------------------------------------------------ - -template -bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps) -{ - namespace comp = helper_functions::comparisons; - - const auto both_empty = Interval::empty(i1) && Interval::empty(i2); - const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); - - const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; - - return both_empty || both_non_empty_equal; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::zero_measure(const Interval & i) -{ - // An empty interval will return false because (NaN == NaN) is false. - return Interval::min(i) == Interval::max(i); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::empty(const Interval & i) -{ - return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::bounds_valid(const Interval & i) -{ - const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - - // Check for emptiness explicitly because it requires both bounds to be NaN - return Interval::empty(i) || is_ordered; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) -{ - const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); - const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); - return lower_contained && upper_contained; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::contains(const Interval & i, const T & value, const T & eps) -{ - return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && - common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); -} - -//------------------------------------------------------------------------------ - -template -T Interval::measure(const Interval & i) -{ - return Interval::max(i) - Interval::min(i); -} - -//------------------------------------------------------------------------------ - -template -Interval Interval::intersect(const Interval & i1, const Interval & i2) -{ - // Construct intersection assuming an intersection exists. - try { - const auto either_empty = Interval::empty(i1) || Interval::empty(i2); - const auto min = std::max(Interval::min(i1), Interval::min(i2)); - const auto max = std::min(Interval::max(i1), Interval::max(i2)); - return either_empty ? Interval() : Interval(min, max); - } catch (...) { - } - - // Otherwise, the intersection is empty. - return Interval(); -} - -//------------------------------------------------------------------------------ - -template -T Interval::clamp_to(const Interval & i, T val) -{ - // clamp val to min - val = std::max(val, Interval::min(i)); - - // clamp val to max - val = std::min(val, Interval::max(i)); - - return Interval::empty(i) ? Interval::NaN : val; -} - -//------------------------------------------------------------------------------ - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp deleted file mode 100644 index 7b8867ca096ae..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file contains a 1D linear lookup table implementation - -#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace helper_functions -{ - -namespace -{ - -/** - * @brief Compute a scaling between 'a' and 'b' - * @note scaling is clamped to be in the interval [0, 1] - */ -template -T interpolate(const T & a, const T & b, U scaling) -{ - static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); - scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); - - const auto m = (b - a); - const auto offset = static_cast(m) * scaling; - return a + static_cast(offset); -} - -// TODO(c.ho) support more forms of interpolation as template functor -// Actual lookup logic, assuming all invariants hold: -// Throw if value is not finite -template -T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) -{ - if (!std::isfinite(value)) { - throw std::domain_error{"Query value is not finite (NAN or INF)"}; - } - if (value <= domain.front()) { - return range.front(); - } else if (value >= domain.back()) { - return range.back(); - } else { - // Fall through to normal case - } - - auto second_idx{0U}; - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (value < domain[idx]) { - second_idx = idx; - break; - } - } - // T must be a floating point between 0 and 1 - const auto num = static_cast(value - domain[second_idx - 1U]); - const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); - const auto t = num / den; - const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); - return static_cast(val); -} - -// Check invariants for table lookup: -template -void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) -{ - if (domain.size() != range.size()) { - throw std::domain_error{"Domain's size does not match range's"}; - } - if (domain.empty()) { - throw std::domain_error{"Empty domain or range"}; - } - // Check if sorted: Can start at 1 because not empty - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 - throw std::domain_error{"Domain is not sorted"}; - } - } -} -} // namespace - -/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. -/// If query value fall out of the domain, then the value at the corresponding edge of the domain is -/// returned. -/// \param[in] domain The domain, or set of x values -/// \param[in] range The range, or set of y values -/// \param[in] value The point in the domain to query, x -/// \return A linearly interpolated value y, corresponding to the query, x -/// \throw std::domain_error If domain or range is empty -/// \throw std::domain_error If range is not the same size as domain -/// \throw std::domain_error If domain is not sorted -/// \throw std::domain_error If value is not finite (NAN or INF) -/// \tparam T The type of the function, must be interpolatable -template -T lookup_1d(const std::vector & domain, const std::vector & range, const T value) -{ - check_table_lookup_invariants(domain, range); - - return lookup_impl_1d(domain, range, value); -} - -/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed -/// into the constructor and not done in the lookup function call -/// \tparam T The type of the function, must be interpolatable -template -class LookupTable1D -{ -public: - /// Constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(const std::vector & domain, const std::vector & range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Move constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(std::vector && domain, std::vector && range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Do a 1D table lookup - /// If query value fall out of the domain, then the value at the corresponding edge of the domain - /// is returned. - /// \param[in] value The point in the domain to query, x - /// \return A linearly interpolated value y, corresponding to the query, x - /// \throw std::domain_error If value is not finite - T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); } - /// Get the domain table - const std::vector & domain() const noexcept { return m_domain; } - /// Get the range table - const std::vector & range() const noexcept { return m_range; } - -private: - std::vector m_domain; - std::vector m_range; -}; // class LookupTable1D - -} // namespace helper_functions -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp deleted file mode 100644 index 8936e2c17d779..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash_config.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ - -/// \brief An implementation of the spatial hash or integer lattice data structure for efficient -/// (O(1)) near neighbor queries. -/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z -/// -/// This implementation can support both 2D and 3D queries -/// (though only one type per data structure), and can support queries of varying radius. This data -/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. -template -class GEOMETRY_PUBLIC SpatialHashBase -{ - using Index3 = details::Index3; - // lint -e{9131} NOLINT There's no other way to make this work in a static assert - static_assert( - std::is_same::value || std::is_same::value, - "SpatialHash only works with Config2d or Config3d"); - -public: - using Hash = std::unordered_multimap; - using IT = typename Hash::const_iterator; - /// \brief Wrapper around an iterator and a distance (from some query point) - class Output - { - public: - /// \brief Constructor - /// \param[in] iterator An iterator pointing to some point - /// \param[in] distance The euclidean distance (2d or 3d) to a reference point - Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance) - { - } - /// \brief Get stored point - /// \return A const reference to the stored point - const PointT & get_point() const { return m_iterator->second; } - /// \brief Get underlying iterator - /// \return A copy of the underlying iterator - IT get_iterator() const { return m_iterator; } - /// \brief Convert to underlying point - /// \return A reference to the underlying point - operator const PointT &() const { return get_point(); } - /// \brief Convert to underlying iterator - /// \return A copy of the iterator - operator IT() const { return get_iterator(); } - /// \brief Get distance to reference point - /// \return The distance - float32_t get_distance() const { return m_distance; } - - private: - IT m_iterator; - float32_t m_distance; - }; // class Output - using OutputVector = typename std::vector; - - /// \brief Constructor - /// \param[in] cfg The configuration object for this class - explicit SpatialHashBase(const ConfigT & cfg) - : m_config{cfg}, - m_hash(), - m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output - m_bins_hit{}, // zero initialization (and below) - m_neighbors_found{} - { - } - - /// \brief Inserts point - /// \param[in] pt The Point to insert - /// \return Iterator pointing to the inserted point - /// \throw std::length_error If the data structure is at capacity - IT insert(const PointT & pt) - { - if (size() >= capacity()) { - throw std::length_error{"SpatialHash: Cannot insert past capacity"}; - } - return insert_impl(pt); - } - - /// \brief Inserts a range of points - /// \param[in] begin The start of the range of points to insert - /// \param[in] end The end of the range of points to insert - /// \tparam IteratorT The iterator type - /// \throw std::length_error If the range of points to insert exceeds the data structure's - /// capacity - template - void insert(IteratorT begin, IteratorT end) - { - // This check is here for strong exception safety - if ((size() + std::distance(begin, end)) > capacity()) { - throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; - } - for (IteratorT it = begin; it != end; ++it) { - (void)insert_impl(*it); - } - } - - /// \brief Removes the specified element from the data structure - /// \param[in] point An iterator pointing to a point to be removed - /// \return An iterator pointing to the element after the erased element - /// \throw std::domain_error If pt is invalid or does not belong to this data structure - /// - /// \note There is no reliable way to check if an iterator is invalid. The checks here are - /// based on a heuristic and is not guaranteed to find all invalid iterators. This method - /// should be used with care and only on valid iterators - IT erase(const IT point) - { - if (end() == m_hash.find(point->first)) { - throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; - } - return m_hash.erase(point); - } - - /// \brief Reset the state of the data structure - void clear() { m_hash.clear(); } - /// \brief Get current number of element stored in this data structure - /// \return Number of stored elements - Index size() const { return m_hash.size(); } - /// \brief Get the maximum capacity of the data structure - /// \return The capacity of the data structure - Index capacity() const { return m_config.get_capacity(); } - /// \brief Whether the hash is empty - /// \return True if data structure is empty - bool8_t empty() const { return m_hash.empty(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT begin() const { return m_hash.begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT end() const { return m_hash.end(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT cbegin() const { return begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT cend() const { return end(); } - - /// \brief Get the number of bins touched during the lifetime of this object, for debugging and - /// size tuning - /// \return The total number of bins touched during near() queries - Index bins_hit() const { return m_bins_hit; } - - /// \brief Get number of near neighbors found during the lifetime of this object, for debugging - /// and size tuning - /// \return The total number of neighbors found during near() queries - Index neighbors_found() const { return m_neighbors_found; } - -protected: - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near_impl( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - // reset output - m_neighbors.clear(); - // Compute bin, bin range - const Index3 ref_idx = m_config.index3(x, y, z); - const float32_t radius2 = radius * radius; - const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); - Index3 idx = idx_range.first; - // For bins in radius - do { // guaranteed to have at least the bin ref_idx is in - // update book-keeping - ++m_bins_hit; - // Iterating in a square/cube pattern is easier than constructing sphere pattern - if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { - // For point in bin - const Index jdx = m_config.index(idx); - const auto range = m_hash.equal_range(jdx); - for (auto it = range.first; it != range.second; ++it) { - const auto & pt = it->second; - const float32_t dist2 = m_config.distance_squared(x, y, z, pt); - if (dist2 <= radius2) { - // Only compute true distance if necessary - m_neighbors.emplace_back(it, sqrtf(dist2)); - } - } - } - } while (m_config.next_bin(idx_range, idx)); - // update book-keeping - m_neighbors_found += m_neighbors.size(); - return m_neighbors; - } - -private: - /// \brief Internal insert method with no error checking - /// \param[in] pt The Point to insert - GEOMETRY_LOCAL IT insert_impl(const PointT & pt) - { - // Compute bin - const Index idx = - m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); - // Insert into bin - return m_hash.insert(std::make_pair(idx, pt)); - } - - const ConfigT m_config; - Hash m_hash; - OutputVector m_neighbors; - Index m_bins_hit; - Index m_neighbors_found; -}; // class SpatialHashBase - -/// \brief The class to be used for specializing on -/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function -/// signatures on 2D and 3D configurations -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash; - -/// \brief Explicit specialization of SpatialHash for 2D configuration -/// \tparam PointT The point type stored in this data structure. -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config2d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius) - { - return this->near_impl(x, y, 0.0F, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. Only the x and y members are respected. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); - } -}; - -/// \brief Explicit specialization of SpatialHash for 3D configuration -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config3d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - return this->near_impl(x, y, z, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius); - } -}; - -template -using SpatialHash2d = SpatialHash; -template -using SpatialHash3d = SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp deleted file mode 100644 index 24c4d6e879d4a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ /dev/null @@ -1,450 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ -/// \brief Index type for identifying bins of the hash/lattice -using Index = std::size_t; -/// \brief Spatial hash functionality not intended to be used by an external user -namespace details -{ -/// \brief Internal struct for packing three indices together -/// -/// The use of this struct publicly is a violation of our coding standards, but I claim it's -/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. -/// This type is needed for conceptual convenience so I don't have massive function parameter -/// lists -struct GEOMETRY_PUBLIC Index3 -{ - Index x; - Index y; - Index z; -}; // struct Index3 - -using BinRange = std::pair; -} // namespace details - -/// \brief The base class for the configuration object for the SpatialHash class -/// \tparam Derived The type of the derived class to support static polymorphism/CRTP -template -class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp -{ -public: - /// \brief Constructor for spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The look up radius - /// \param[in] capacity The maximum number of points the spatial hash can store - Config( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) - : m_min_x{min_x}, - m_min_y{min_y}, - m_min_z{min_z}, - m_max_x{max_x}, - m_max_y{max_y}, - m_max_z{max_z}, - m_side_length{radius}, - m_side_length2{radius * radius}, - m_side_length_inv{1.0F / radius}, - m_capacity{capacity}, - m_max_x_idx{check_basis_direction(min_x, max_x)}, - m_max_y_idx{check_basis_direction(min_y, max_y)}, - m_max_z_idx{check_basis_direction(min_z, max_z)}, - m_y_stride{m_max_x_idx + 1U}, - m_z_stride{m_y_stride * (m_max_y_idx + 1U)} - { - if (radius <= 0.0F) { - throw std::domain_error("Error constructing SpatialHash: must have positive side length"); - } - - if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // small fudging to prevent weird boundary effects - // (e.g (x=x_max, y) rolls index over to (x=0, y+1) - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - // lint -e{1938} read only access is fine NOLINT - m_max_x -= FEPS; - m_max_y -= FEPS; - m_max_z -= FEPS; - if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // I don't know if this is even possible given other checks - if (std::numeric_limits::max() == m_max_z_idx) { - throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); - } - } - - /// \brief Given a reference index triple, compute the first and last bin - /// \param[in] ref The reference index triple - /// \param[in] radius The allowable radius for any point in the reference bin to any point in the - /// range - /// \return A pair where the first element is an index triple of the first bin, and the second - /// element is an index triple for the last bin - details::BinRange bin_range(const details::Index3 & ref, const float radius) const - { - // Compute distance in units of voxels - const Index i_radius = static_cast(std::ceil(radius / m_side_length)); - // Dumb ternary because potentially unsigned Index type - const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; - const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; - const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; - // In 2D mode, m_max_z should be 0, same with ref.z - const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); - const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); - const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); - // return bottom-left portion of cube and top-right portion of cube - return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; - } - - /// \brief Get next index within a given range - /// \return True if idx is valid and still in range - /// \param[in] range The max and min bin indices - /// \param[inout] idx The index to be incremented, updated even if a negative result occurs - bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const - { - // TODO(c.ho) is there any way to make this neater without triple nested if? - bool8_t ret = true; - ++idx.x; - if (idx.x > range.second.x) { - idx.x = range.first.x; - ++idx.y; - if (idx.y > range.second.y) { - idx.y = range.first.y; - ++idx.z; - if (idx.z > range.second.z) { - ret = false; - } - } - } - return ret; - } - /// \brief Get the maximum capacity of the spatial hash - /// \return The capacity - Index get_capacity() const { return m_capacity; } - - /// \brief Getter for the side length, equivalently the lookup radius - float32_t radius2() const { return m_side_length2; } - - //////////////////////////////////////////////////////////////////////////////////////////// - // "Polymorphic" API - /// \brief Compute the single index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The combined index of the bin for a given point - Index bin(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().bin_(x, y, z); - } - /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points - /// such that their distance is within a certain threshold - /// \param[in] ref The index triple of the bin containing the reference point - /// \param[in] query The index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if query and ref could possibly hold points within reference distance to one - /// another - bool is_candidate_bin( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const - { - return this->impl().valid(ref, query, ref_distance2); - } - /// \brief Compute the decomposed index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().index3_(x, y, z); - } - /// \brief Compute the composed single index given a decomposed index - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index(const details::Index3 & idx) const { return this->impl().index_(idx); } - /// \brief Compute the squared distance between the two points - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d or 3d) - template - float32_t distance_squared( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - return this->impl().distance_squared_(x, y, z, pt); - } - -protected: - /// \brief Computes the index in the x basis direction - /// \param[in] x The x value of a point - /// \return The x offset of the index - Index x_index(const float32_t x) const - { - return static_cast( - std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); - } - /// \brief Computes the index in the y basis direction - /// \param[in] y The y value of a point - /// \return The x offset of the index - Index y_index(const float32_t y) const - { - return static_cast( - std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); - } - /// \brief Computes the index in the z basis direction - /// \param[in] z The z value of a point - /// \return The x offset of the index - Index z_index(const float32_t z) const - { - return static_cast( - std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); - } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const - { - return bin_impl(xdx, ydx) + (zdx * m_z_stride); - } - - /// \brief The index offset of a point given it's x and y values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash - Index bin_impl(const float32_t x, const float32_t y) const - { - return bin_impl(x_index(x), y_index(y)); - } - /// \brief The index of a point given it's x, y and z values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const - { - return bin_impl(x, y) + (z_index(z) * m_z_stride); - } - /// \brief The distance between two indices as a float, where adjacent indices have zero - /// distance (e.g. dist(0, 1) = 0) - float32_t idx_distance(const Index ref_idx, const Index query_idx) const - { - /// Not using fabs because Index is (possibly) unsigned - const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(i_dist) - 1.0F; - return std::max(dist, 0.0F); - } - - /// \brief Get side length squared - float side_length2() const { return m_side_length2; } - -private: - /// \brief Sanity check a range in a basis direction - /// \return The number of voxels in the given basis direction - Index check_basis_direction(const float32_t min, const float32_t max) const - { - if (min >= max) { - throw std::domain_error("SpatialHash::Config: must have min < max"); - } - // This family of checks is to ensure that you don't get weird casting effects due to huge - // floating point values - const float64_t dmax = static_cast(max); - const float64_t dmin = static_cast(min); - const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); - if (flt_max <= width) { - throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); - } - return static_cast(width); - } - float32_t m_min_x; - float32_t m_min_y; - float32_t m_min_z; - float32_t m_max_x; - float32_t m_max_y; - float32_t m_max_z; - float32_t m_side_length; - float32_t m_side_length2; - float32_t m_side_length_inv; - Index m_capacity; - Index m_max_x_idx; - Index m_max_y_idx; - Index m_max_z_idx; - Index m_y_stride; - Index m_z_stride; -}; // class Config - -/// \brief Configuration class for a 2d spatial hash -class GEOMETRY_PUBLIC Config2d : public Config -{ -public: - /// \brief Config constructor for 2D spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 2d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 2D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 2d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 2d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 2d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - (void)z; - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - return (dx * dx) + (dy * dy); - } -}; // class Config2d - -/// \brief Configuration class for a 3d spatial hash -class GEOMETRY_PUBLIC Config3d : public Config -{ -public: - /// \brief Config constructor for a 3d spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 3d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 3D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 3d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 3d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 3d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (3d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - const float32_t dz = z - point_adapter::z_(pt); - return (dx * dx) + (dy * dy) + (dz * dz); - } -}; // class Config3d -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp deleted file mode 100644 index 8972246997997..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml deleted file mode 100644 index f53412298a485..0000000000000 --- a/common/autoware_auto_geometry/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_geometry - 1.0.0 - Geometry related algorithms - Satoshi Ota - Apache License 2.0 - - Apex.AI, Inc. - - ament_cmake_auto - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs - geometry_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - osrf_testing_tools_cpp - - - ament_cmake - - diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp deleted file mode 100644 index 3a4ea96a151a2..0000000000000 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -// cspell: ignore eigenbox -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -//////////////////////////////////////////////////////////////////////////////// -void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) -{ - ret.x = std::max( - norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); - ret.y = std::max( - norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); -} -//////////////////////////////////////////////////////////////////////////////// -void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) -{ - (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); - // orientation: this is robust to lines - const float32_t yaw_rad_2 = - atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; - box.orientation.w = cosf(yaw_rad_2); - box.orientation.z = sinf(yaw_rad_2); - // centroid - box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); -} - -autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::Shape retval; - // Polygon is 2D rectangle - geometry_msgs::msg::Polygon polygon; - auto & points = polygon.points; - points.resize(4); - - // Note that the x and y of size field in BoundingBox should be swapped when being used to - // compute corners. - // origin of reference system: centroid of bbox - points[0].x = -0.5F * box.size.y; - points[0].y = -0.5F * box.size.x; - points[0].z = -box.size.z; - - points[1] = points[0]; - points[1].y += box.size.x; - - points[2] = points[1]; - points[2].x += box.size.y; - - points[3] = points[2]; - points[3].y -= box.size.x; - - retval.footprint = polygon; - retval.dimensions.z = 2 * box.size.z; - - return retval; -} - -autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::DetectedObject ret; - - ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); - ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); - ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); - ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); - ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); - ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); - ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); - ret.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - - ret.shape = make_shape(box); - - ret.existence_probability = 1.0F; - - autoware_auto_perception_msgs::msg::ObjectClassification label; - label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - label.probability = 1.0F; - ret.classification.emplace_back(std::move(label)); - - return ret; -} - -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) -{ - std::vector retval(shape_msg.footprint.points.size()); - geometry_msgs::msg::TransformStamped tf; - tf.transform.rotation = orientation; - tf.transform.translation.x = centroid.x; - tf.transform.translation.y = centroid.y; - tf.transform.translation.z = centroid.z; - - for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { - tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); - } - return retval; -} - -} // namespace details -/////////////////////////////////////////////////////////////////////////////// -// pre-compilation -using autoware::common::types::PointXYZIF; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using PointXYZIFVIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); -template BoundingBox lfit_bounding_box_2d( - const PointXYZIFVIT begin, const PointXYZIFVIT end); -using geometry_msgs::msg::Point32; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using Point32VIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); -template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp deleted file mode 100644 index ffd91aaa08b3a..0000000000000 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace spatial_hash -{ -//////////////////////////////////////////////////////////////////////////////// -Config2d::Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return bin_impl(x, y); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config2d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return {x_index(x), y_index(y), Index{}}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y); -} -//////////////////////////////////////////////////////////////////////////////// -Config3d::Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - return bin_impl(x, y, z); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config3d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - const float dz = idx_distance(ref.z, query.z); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - return {x_index(x), y_index(y), z_index(z)}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y, idx.z); -} -//////////////////////////////////////////////////////////////////////////////// -template class SpatialHash; -template class SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp deleted file mode 100644 index a179fbde5f151..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ /dev/null @@ -1,550 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_BOUNDING_BOX_HPP_ -#define TEST_BOUNDING_BOX_HPP_ - -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::geometry::point_adapter::yr_; -using autoware_auto_perception_msgs::msg::BoundingBox; - -template -class BoxTest : public ::testing::Test -{ -protected: - std::list points; - BoundingBox box; - - void minimum_area_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - void minimum_perimeter_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - // cspell: ignore eigenbox - template - void eigenbox(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - template - void lfit_bounding_box_2d(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - - PointT make(const float x, const float y) - { - PointT ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; - } - - void check(const float cx, const float cy, const float sx, const float sy, const float val) const - { - constexpr float32_t TOL = 1.0E-4F; - ASSERT_LT(fabsf(box.size.x - sx), TOL); - ASSERT_LT(fabsf(box.size.y - sy), TOL); - ASSERT_LT(fabsf(box.value - val), TOL) << box.value; - - ASSERT_LT(fabsf(box.centroid.x - cx), TOL); - ASSERT_LT(fabsf(box.centroid.y - cy), TOL); - } - - void test_corners(const std::vector & expect, const float TOL = 1.0E-6F) const - { - for (uint32_t idx = 0U; idx < 4U; ++idx) { - bool found = false; - for (auto & p : expect) { - if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { - found = true; - break; - } - } - ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; - } - } - - /// \brief th_deg - phi_deg, normalized to +/- 180 deg - float32_t angle_distance_deg(const float th_deg, const float phi_deg) const - { - return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; - } - - /// \brief converts a radian value to a degree value - float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; } - - void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const - { - bool found = false; - const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; - ASSERT_TRUE(found) << angle_deg; - } -}; // BoxTest - -// Instantiate tests for given types, add more types here as they are used -using PointTypesBoundingBox = - ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// TODO(c.ho) consider typed and parameterized tests: -// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test - -/////////////////////////////////////////// -TYPED_TEST(BoxTest, PointSegmentDistance) -{ - using autoware::common::geometry::closest_segment_point_2d; - using autoware::common::geometry::point_line_segment_distance_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); - // boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -2.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), 1.0F); - ASSERT_FLOAT_EQ(y_(t), 5.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); -} - -TYPED_TEST(BoxTest, ClosestPointOnLine) -{ - using autoware::common::geometry::closest_line_point_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - // out-of-boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -5.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); -} - -TYPED_TEST(BoxTest, Basic) -{ - const std::vector data{ - {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; - this->points.insert(this->points.begin(), data.begin(), data.end()); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_corners(data); - this->test_orientation(0.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); - this->test_corners(data); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, OrientedTriangle) -{ - this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); - - this->minimum_area_bounding_box(); - - this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); - this->test_orientation(45.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); - this->test_orientation(0.0F); -} -// -TYPED_TEST(BoxTest, Hull) -{ - const uint32_t FUZZ_SIZE = 1024U; - const float dx = 9.0F; - const float dy = 15.0F; - const float rx = 10.0F; - const float ry = 5.0F; - const float dth = 0.0F; - - ASSERT_EQ(FUZZ_SIZE % 4U, 0U); - - // fuzz part 1 - for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { - const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; - this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); - } - - this->minimum_area_bounding_box(); - - // allow 10cm = 2% of size for tolerance - const float TOL_M = 0.1F; - ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); - ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); - - this->test_corners( - {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry), - this->make(dx + rx, dy - ry)}, - TOL_M); - - this->test_orientation(this->rad2deg(dth), 1.0F); - // allow 1 degree of tolerance - - ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); - ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); - ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); -} - -// -TYPED_TEST(BoxTest, Collinear) -{ - this->points.insert( - this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2), - this->make(-2, -4), this->make(-3, -4), this->make(-4, -4), - this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)}); - - this->minimum_area_bounding_box(); - - this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); - this->test_corners( - {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, Line1) -{ - this->points.insert( - this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9), - this->make(-16, 12), this->make(-20, 15), this->make(-24, 18), - this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); - - this->minimum_perimeter_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); -} - -// -TYPED_TEST(BoxTest, Line2) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0), - this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); - this->test_orientation(0.0F); - this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); -} - -// -TYPED_TEST(BoxTest, Line3) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15), - this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, 4))); - this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); -} - -/* _ - /_/ <-- first guess is suboptimal - -*/ -TYPED_TEST(BoxTest, SuboptimalInit) -{ - this->points.insert( - this->points.begin(), - {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)}); - - this->minimum_area_bounding_box(); - - this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); - this->test_orientation(this->rad2deg(atan2f(15, 8))); - // these are approximate. - this->test_corners( - {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F), - this->make(13.2353F, -7.05882F)}, - 0.1F); -} - -// -TYPED_TEST(BoxTest, Centered) -{ - this->points.insert( - this->points.begin(), - {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); - this->test_orientation(45.0F); - this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); -} - -// convex_hull is imperfect in this case, check if this can save the day -TYPED_TEST(BoxTest, OverlappingPoints) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1), - this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_orientation(0.0F); - this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); -} - -// check that minimum perimeter box is different from minimum area box -TYPED_TEST(BoxTest, Perimeter) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3), - this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal - this->make(2, 0), this->make(3, 0)}); - - this->minimum_area_bounding_box(); - - this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); - this->test_orientation(-53.13F, 0.001F); - this->test_corners( - {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)}); - - // eigenbox should produce AABB TODO(c.ho) - // compute minimum perimeter box - this->minimum_perimeter_bounding_box(); - this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); - // perimeter for first box would be 14.8 - this->test_orientation(0.0F, 0.001F); - this->test_corners( - {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)}); -} - -// bounding box is diagonal on an L -TYPED_TEST(BoxTest, Eigenbox1) -{ - std::vector v{this->make(0, 0), this->make(0, 1), - this->make(-1, 2), // small fudge to force diagonal - this->make(0, 3), this->make(0, 4), - this->make(1, 0), this->make(2, -1), // small fudge to force diagonal - this->make(3, 0), this->make(4, 0)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - // rotating calipers should produce a diagonal box - this->minimum_area_bounding_box(); - const float r = 4.0F; - - this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); - const std::vector diag_corners{ - this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - - //// perimeter should also produce diagonal //// - this->minimum_perimeter_bounding_box(); - this->check( - 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// eigenbox should also produce aabb //// - this->eigenbox(v.begin(), v.end()); - // TODO(c.ho) don't care about value.. - this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// Lfit should produce aabb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - - this->test_corners( - {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); - this->test_orientation(0.0F, 3.0F); -} - -// same as above test, just rotated -TYPED_TEST(BoxTest, Eigenbox2) -{ - std::vector v{this->make(0, 0), this->make(1, 1), - this->make(1, 2), // small fudge to force diagonal - this->make(3, 3), this->make(4, 4), - this->make(1, -1), this->make(1, -2), // small fudge to force diagonal - this->make(3, -3), this->make(4, -4)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - const std::vector diag_corners{ - this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; - // rotating calipers should produce a aabb - this->minimum_area_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 32); - this->test_corners(diag_corners); - this->test_orientation(0.0F, 0.001F); - - //// perimeter should also produce aabb //// - this->minimum_perimeter_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 12); - this->test_corners(diag_corners); - - //// eigenbox should also produce obb //// - this->eigenbox(v.begin(), v.end()); - this->test_orientation(0.0F, 0.001F); - this->test_corners(diag_corners); - //// Lfit should produce obb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); - this->test_orientation(45.0F, 2.0F); -} -// line case for eigenbox -TYPED_TEST(BoxTest, Eigenbox3) -{ - // horizontal line with some noise - std::vector v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02), - this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01), - this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)}; - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); - this->test_orientation(0.0F, 1.0F); -} - -// bad case: causes intersection2d to fail -// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases -TYPED_TEST(BoxTest, IntersectFail) -{ - std::vector vals{ - this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003), - this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5), - this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3), - this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3), - this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3), - this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3), - this->make(1.7, 10.3)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1), - this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5), - this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); -} -/// Handle slight floating point underflow case -// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to -// tiny differences in floating point math when using our compile flags -TYPED_TEST(BoxTest, EigUnderflow) -{ - using autoware::common::geometry::bounding_box::details::Covariance2d; - // auto discriminant = [](const Covariance2d cov) -> float32_t { - // // duplicated raw math - // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // return (tr_2 * tr_2) - det; - // }; - TypeParam u, v; - const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; - // EXPECT_LT(discriminant(c1), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); - const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; - // EXPECT_LT(discriminant(c2), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); - const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; - // EXPECT_LT(discriminant(c3), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); - const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; - // EXPECT_LT(discriminant(c4), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); - const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; - // EXPECT_LT(discriminant(c5), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); - const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; - // EXPECT_LT(discriminant(c6), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); -} - -//////////////////////////////////////////////// - -#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp deleted file mode 100644 index fc2d97c257b95..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ /dev/null @@ -1,260 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_SPATIAL_HASH_HPP_ -#define TEST_SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include - -#include -#include - -using autoware::common::geometry::spatial_hash::Config2d; -using autoware::common::geometry::spatial_hash::Config3d; -using autoware::common::geometry::spatial_hash::SpatialHash; -using autoware::common::geometry::spatial_hash::SpatialHash2d; -using autoware::common::geometry::spatial_hash::SpatialHash3d; -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedSpatialHashTest : public ::testing::Test -{ -public: - TypedSpatialHashTest() : ref(), EPS(0.001F) - { - ref.x = 0.0F; - ref.y = 0.0F; - ref.z = 0.0F; - } - -protected: - template - void add_points( - SpatialHash & hash, const uint32_t points_per_ring, const uint32_t num_rings, - const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F) - { - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - - // insert - float32_t r = dr; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) + dx; - pt.y = r * sinf(th) + dy; - pt.z = 0.0F; - hash.insert(pt); - th += dth; - } - r += dr; - } - } - PointT ref; - const float32_t EPS; -}; // SpatialHash -// test struct - -// Instantiate tests for given types, add more types here as they are used -using PointTypesSpatialHash = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -/////////////////////////////////////////////////////////////// -// TODO(christopher.ho) helper functions to simplify this stuff -/// all points in one bin -TYPED_TEST(TypedSpatialHashTest, OneBin) -{ - using PointT = TypeParam; - const float32_t dr = 1.0F; - Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 10U; - const uint32_t NUM_RINGS = 1U; - this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); - - // loop through all points - float r = dr - this->EPS; - for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { - const uint32_t n_pts = rdx * PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - ASSERT_EQ(points_seen, n_pts); - r += dr; - // Make sure statistics are consistent - EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); - EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); - } - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(PTS_PER_RING, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} -/// test out of bounds points -TYPED_TEST(TypedSpatialHashTest, Oob) -{ - using PointT = TypeParam; - const float32_t dr = 20.0F; - Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 12U; - this->add_points(hash, PTS_PER_RING, 1U, dr); - - // loop through all points - float32_t r = dr + this->EPS; - const uint32_t n_pts = PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); -} -// 3d test case -TYPED_TEST(TypedSpatialHashTest, 3d) -{ - using PointT = TypeParam; - Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; - SpatialHash3d hash{cfg}; - EXPECT_TRUE(hash.empty()); - - // build concentric rings around origin - const uint32_t points_per_ring = 32U; - const uint32_t num_rings = 5U; - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - std::vector pts{}; - - // insert - const float32_t r = 10.0F; - float32_t phi = 0.0f; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) * cosf(phi); - pt.y = r * sinf(th) * cosf(phi); - pt.z = r * sinf(phi); - pts.push_back(pt); - th += dth; - } - hash.insert(pts.begin(), pts.end()); - pts.clear(); - phi += 1.0f; - } - EXPECT_FALSE(hash.empty()); - EXPECT_EQ(hash.size(), num_rings * points_per_ring); - - // loop through all points - const uint32_t n_pts = num_rings * points_per_ring; - const auto & neighbors = hash.near(this->ref, r + this->EPS); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); - ASSERT_LT(dist, r + this->EPS); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); - - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(n_pts, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} - -/// edge cases -TEST(SpatialHashConfig, BadCases) -{ - // negative side length - EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); - // min_x >= max_x - EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_y >= max_y - EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_z >= max_z - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // floating point limit - constexpr float32_t max_float = std::numeric_limits::max(); - EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), - std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), - std::domain_error); - // y would overflow - // constexpr float32_t big_float = - // static_cast(std::numeric_limits::max() / 4UL); - // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), - // std::domain_error); - // z would overflow - // EXPECT_THROW( - // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), - // std::domain_error); - // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow -} -#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp deleted file mode 100644 index 81865656c55b5..0000000000000 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/lookup_table.hpp" - -#include - -#include - -#include -#include - -using autoware::common::helper_functions::interpolate; -using autoware::common::helper_functions::lookup_1d; -using autoware::common::helper_functions::LookupTable1D; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class BadCase : public ::testing::Test -{ -}; - -using BadTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BadCase, BadTypes, ); - -// Empty domain/range -TYPED_TEST(BadCase, Empty) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); -} - -// Unequal domain/range -TYPED_TEST(BadCase, UnequalDomain) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); -} - -// Not sorted domain -TYPED_TEST(BadCase, DomainNotSorted) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); -} - -//////////////////////////////////////////////////////////////////////////////// - -template -class SanityCheck : public ::testing::Test -{ -public: - using T = Type; - - void SetUp() override - { - domain_ = std::vector{T{1}, T{3}, T{5}}; - range_ = std::vector{T{2}, T{4}, T{0}}; - ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); - } - - bool not_in_domain(const T bad_value) const noexcept - { - return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); - } - - void check(const T expected, const T actual) const noexcept - { - if (std::is_floating_point::value) { - EXPECT_FLOAT_EQ(actual, expected); - } else { - EXPECT_EQ(actual, expected); - } - } - -protected: - std::vector domain_{}; - std::vector range_{}; - std::unique_ptr> table_{}; -}; - -using NormalTypes = ::testing::Types; -TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); - -TYPED_TEST(SanityCheck, Exact) -{ - const auto x = this->domain_[1U]; - const auto result = this->table_->lookup(x); - ASSERT_FALSE(this->not_in_domain(x)); - this->check(result, this->range_[1U]); -} - -TYPED_TEST(SanityCheck, Interpolation) -{ - const auto x = TypeParam{2}; - // Assert x is not identically in domain_ - ASSERT_TRUE(this->not_in_domain(x)); - const auto result = this->table_->lookup(x); - this->check(result, TypeParam{3}); -} - -TYPED_TEST(SanityCheck, AboveRange) -{ - const auto x = TypeParam{999999}; - ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.back()); -} - -TYPED_TEST(SanityCheck, BelowRange) -{ - const auto x = TypeParam{0}; - ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.front()); -} - -TEST(LookupTableHelpers, Interpolate) -{ - { - const auto scaling = 0.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = -1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 2.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = 0.75f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); - } -} - -// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp deleted file mode 100644 index bc9c28682ed44..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 Apex.AI, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include - -#include -#include -#include - -template -class AreaTest : public ::testing::Test -{ -protected: - DataStructure data_{}; - using Point = typename DataStructure::value_type; - using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); - - auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); } - - void add_point(Real x, Real y) - { - namespace pa = autoware::common::geometry::point_adapter; - Point p{}; - pa::xr_(p) = x; - pa::yr_(p) = y; - (void)data_.insert(data_.end(), p); - } -}; - -// Data structures to test... -template -using TestTypes_ = ::testing::Types..., std::list...>; -// ... and point types to test -using TestTypes = TestTypes_; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(AreaTest, TestTypes, ); - -// The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) -{ - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An individual point has zero area -TYPED_TEST(AreaTest, DegenerateOne) -{ - this->add_point(0.0, 0.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An line segment has zero area -TYPED_TEST(AreaTest, DegenerateTwo) -{ - this->add_point(1.0, -1.0); - this->add_point(-3.0, 2.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// Simple triangle -TYPED_TEST(AreaTest, Triangle) -{ - this->add_point(1.0, 0.0); - this->add_point(3.0, 0.0); // 2.0 wide - this->add_point(2.0, 2.0); // 2.0 tall - EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h -} - -// Rectangle is easy to do computational -TYPED_TEST(AreaTest, Rectangle) -{ - this->add_point(-5.0, -5.0); - this->add_point(-2.0, -5.0); // L = 3 - this->add_point(-2.0, -1.0); // H = 4 - this->add_point(-5.0, -1.0); - EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h -} - -// Parallelogram is slightly less trivial than a rectangle -TYPED_TEST(AreaTest, Parallelogram) -{ - this->add_point(-5.0, 1.0); - this->add_point(-2.0, 1.0); // L = 3 - this->add_point(-1.0, 3.0); // H = 2 - this->add_point(-4.0, 3.0); - EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h -} - -// Octagon is analytical and reasonably easy to build -TYPED_TEST(AreaTest, Octagon) -{ - const auto sq2 = std::sqrt(2.0); - const auto a = 1.0; - const auto a2 = a / 2.0; - const auto b = (a + sq2) / 2.0; - this->add_point(-a2, -b); - this->add_point(a2, -b); - this->add_point(b, -a2); - this->add_point(b, a2); - this->add_point(a2, b); - this->add_point(-a2, b); - this->add_point(-b, a2); - this->add_point(-b, -a2); - const auto expect = (2.0 * (1.0 + sq2)) * (a * a); - EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h -} - -// Bad case -TYPED_TEST(AreaTest, NotCcw) -{ - this->add_point(0.0, 0.0); - this->add_point(1.0, 1.0); - this->add_point(1.0, 0.0); - this->add_point(2.0, 1.0); - EXPECT_THROW(this->area(), std::domain_error); -} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp deleted file mode 100644 index baf6967edd47e..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include -#include - -#include - -#include -#include - -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::yr_; - -// Helper function for adding new points -template -T make_points(const float x, const float y) -{ - T ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; -} - -// PointTypes to be tested -using PointTypes = - ::testing::Types; - -// Wrapper function for stubbing output of -// autoware::common::geometry::check_point_position_to_line_2d -template -int point_position_checker(const T & p1, const T & p2, const T & q) -{ - auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); - if (result > 0.0F) { - return 1; - } else if (result < 0.0F) { - return -1; - } - return result; -} - -// Templated struct defining the function parameters for -// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for -// assertion of return values from the function -template -struct PointPositionToLine : public ::testing::Test -{ - struct Parameters - { - T p1; - T p2; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(PointPositionToLine); - -template -std::vector::Parameters, int>> - PointPositionToLine::input_output{ - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, -1}, - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, 1}, - // Check point on the line - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, 0}, - }; - -TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) -{ - for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { - const auto & input = PointPositionToLine::input_output[i].first; - EXPECT_EQ( - point_position_checker(input.p1, input.p2, input.q), - PointPositionToLine::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); - -///////////////////////////////////////////////////////////////////////////////////// - -// Templated struct defining the function parameters for -// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for -// assertion of return values from the function -template -struct InsidePolygon : public ::testing::Test -{ - struct Parameters - { - std::vector polygon; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(InsidePolygon); - -template -std::vector::Parameters, bool>> InsidePolygon::input_output{ - // point inside the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.F, 0.5F)}, - true}, - // point below the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.F)}, - false}, - // point above the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 1.75F)}, - false}, - // point on the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.5F)}, - true}, -}; - -TYPED_TEST_P(InsidePolygon, InsidePolygonTest) -{ - for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { - const auto & input = InsidePolygon::input_output[i].first; - EXPECT_EQ( - autoware::common::geometry::is_point_inside_polygon_2d( - input.polygon.begin(), input.polygon.end(), input.q), - InsidePolygon::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); - -TEST(ordered_check, basic) -{ - // CW - std::vector points_list = { - make_points(8.0, 4.0), - make_points(9.0, 1.0), - make_points(3.0, 2.0), - make_points(2.0, 5.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // CCW - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(9.0, 1.0), - make_points(8.0, 4.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(8.0, 4.0), - make_points(9.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(0.0, 0.0), - make_points(1.0, 1.0), - make_points(1.0, 0.0), - make_points(2.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Collinearity - points_list = { - make_points(2.0, 2.0), - make_points(4.0, 4.0), - make_points(6.0, 6.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); -} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp deleted file mode 100644 index 8b9bbce36c428..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" - -#include - -#include - -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedConvexHullTest : public ::testing::Test -{ -protected: - std::list list; - - typename std::list::const_iterator convex_hull() - { - const auto ret = autoware::common::geometry::convex_hull(list); - return ret; - } - - void check_hull( - const typename std::list::const_iterator last, const std::vector & expect, - const bool8_t strict = true) - { - uint32_t items = 0U; - for (auto & pt : expect) { - bool8_t found = false; - auto it = list.begin(); - while (it != last) { - constexpr float32_t TOL = 1.0E-6F; - if ( - fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL && - (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict - { - found = true; - break; - } - ++it; - } - ASSERT_TRUE(found) << items; - ++items; - } - if (strict) { - ASSERT_EQ(items, expect.size()); - } - } - - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class convex_hull_test - -// Instantiate tests for given types, add more types here as they are used -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -////////////////////////////////////////// - -/* - 3 - 2 -1 -*/ -TYPED_TEST(TypedConvexHullTest, Triangle) -{ - std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 3U); - // check order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 3); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} -/* - 2 1 - -4 - 3 -*/ -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadrilateral) -{ - std::vector expect( - {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, -6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, -2); - ++it; // node 3 - ASSERT_FLOAT_EQ(it->x, -1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, -5); - ++it; // node 2 - ASSERT_EQ(it, last); -} - -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, QuadHull) -{ - std::vector data( - {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), - this->make(6, 5, 5)}); - std::vector expect{{data[0], data[1], data[2], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 5); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// a ring plus a bunch of random stuff in the middle -TYPED_TEST(TypedConvexHullTest, Hull) -{ - const uint32_t HULL_SIZE = 13U; - const uint32_t FUZZ_SIZE = 50U; - const float32_t dth = 1.133729384F; // some weird irrational(ish) number - const float32_t r_hull = 20.0F; - const float32_t r_fuzz = 10.0F; - ASSERT_LT(r_fuzz, r_hull); - - std::vector hull; - - uint32_t hull_pts = 0U; - float32_t th = 0.0F; - // hull part 1 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 1 - uint32_t fuzz_pts = 0U; - for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++fuzz_pts; - } - - // hull part 2 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 2 - for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - // hull part 3 - for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - const auto last = this->convex_hull(); - - this->check_hull(last, hull); - ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); -} - -TYPED_TEST(TypedConvexHullTest, Collinear) -{ - std::vector data( - {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4), - this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5), - this->make(1, 1, 0)}); - const std::vector expect{{data[0], data[2], data[3], data[5]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 7); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->z, 4); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// degenerate cases -TYPED_TEST(TypedConvexHullTest, OverlappingPoints) -{ - std::vector data( - {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4), - this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5), - this->make(3, -1, 0)}); - const std::vector expect{{data[0], data[1], data[2]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); - this->check_hull(last, expect, false); -} - -TYPED_TEST(TypedConvexHullTest, Line) -{ - std::vector data( - {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4), - this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5), - this->make(-11, 11, 0)}); - const std::vector expect{{data[2], data[7]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); - this->check_hull(last, expect, false); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 5); - ++it; // node 8 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -/* -1 - 4 - - 3 - 2 -*/ -TYPED_TEST(TypedConvexHullTest, LowerHull) -{ - const std::vector data({ - this->make(1, 3, 1), - this->make(2, -2, 2), - this->make(3, -1, 3), - this->make(4, 1, 4), - }); - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - this->check_hull(last, data); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_EQ(it, last); -} - -// Ensure the leftmost item is properly shuffled -/* - 5 -1 6 - 2 4 - 3 -*/ -TYPED_TEST(TypedConvexHullTest, Root) -{ - const std::vector data({ - this->make(0, 0, 1), - this->make(1, -1, 2), - this->make(3, -2, 3), - this->make(4, 0, 4), - this->make(3, 1, 5), - this->make(1, 0, 6), - }); - const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); - this->check_hull(last, expect); - - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_FLOAT_EQ(it->z, 5); - ++it; - ASSERT_EQ(it, last); - EXPECT_NE(last, this->list.cend()); - EXPECT_EQ(last->z, 6); -} - -// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp deleted file mode 100644 index 0a7dd288d0d79..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_geometry.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "gtest/gtest.h" -#include "test_bounding_box.hpp" -#include "test_spatial_hash.hpp" - -int32_t main(int32_t argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp deleted file mode 100644 index 9477a83a488ed..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/hull_pockets.hpp" - -#include - -#include - -#include -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedHullPocketsTest : public ::testing::Test -{ -protected: - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class TypedHullPocketsTest - -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// Inner test function, shared among all the tests -template -typename std::vector::value_type>> -compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end) -{ - auto convex_hull_list = - std::list::value_type>{polygon_start, polygon_end}; - const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list); - - const typename decltype(convex_hull_list)::const_iterator list_beginning = - convex_hull_list.begin(); - const auto pockets = autoware::common::geometry::hull_pockets( - polygon_start, polygon_end, list_beginning, cvx_hull_end); - - return pockets; -} - -// Test for a triangle - any triangle should really not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Triangle) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +--------------------+ -// | | -// | | -// | | -// | | -// | | -// | | -// +--------------------+ -// This should not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Box) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +-----+ +-----+ -// / | | | -// / | | | -// + | | + -// | | | | -// | | | | -// | -------------- | -// | | -// | | -// | | -// | | -// +------------------------------+ -// This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, UShape) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), - this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 4u); - ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); - ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); -} - -// Test for the use case: -// +------+ -// | | -// | | -// | | -// +------------------+ +------+ -// | | -// | | -// | | -// +--------------------------------+ -// -// This should come up with two pockets, a triangle on the top left and one on the -// top right. -TYPED_TEST(TypedHullPocketsTest, TypicalGap) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0), - this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 2u); - ASSERT_EQ(pockets[0].size(), 3u); - ASSERT_EQ(pockets[1].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} - -// Test for the use case: -// -// +-----------------+ -// | | -// | | -// + | -// / | -// / | -// +-----------------+ -// -// This should come up with one pocket, in particular a pocket that contains -// the segment of the final to the first point. -TYPED_TEST(TypedHullPocketsTest, EndsInPocket) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0), - this->make(0, 2, 0), this->make(0.1, 1, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp deleted file mode 100644 index 1036c69573c49..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2021 Apex.AI, 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. - -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/intersection.hpp" - -#include - -#include - -struct TestPoint -{ - autoware::common::types::float32_t x; - autoware::common::types::float32_t y; -}; - -struct IntersectionTestParams -{ - std::list polygon1_pts; - std::list polygon2_pts; - std::list expected_intersection_pts; -}; - -void order_ccw(std::list & points) -{ - const auto end_it = autoware::common::geometry::convex_hull(points); - ASSERT_EQ(end_it, points.end()); // Points should have represent a shape -} - -class IntersectionTest : public ::testing::TestWithParam -{ -}; - -TEST_P(IntersectionTest, Basic) -{ - const auto get_ordered_polygon = [](auto polygon) { - order_ccw(polygon); - return polygon; - }; - - const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); - const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); - const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); - - const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - - ASSERT_EQ(result.size(), expected_intersection.size()); - auto expected_shape_it = expected_intersection.begin(); - for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { - EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); - EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); - ++expected_shape_it; - } -} - -INSTANTIATE_TEST_SUITE_P( - Basic, IntersectionTest, - ::testing::Values( - IntersectionTestParams{{}, {}, {}}, - IntersectionTestParams{// Partial intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, - {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}}, - // Full intersection with overlapping edges - // TODO(yunus.caliskan): enable after #1231 - // IntersectionTestParams{ - // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // }, - IntersectionTestParams{ - // Fully contained - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - }, - IntersectionTestParams{ - // Fully contained triangle - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - }, - IntersectionTestParams{// Triangle rectangle intersection. - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}}, - IntersectionTestParams{// No intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, - {}} // cppcheck-suppress syntaxError - )); - -TEST(PolygonPointTest, Basic) -{ - GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 - std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; - order_ccw(polygon); - EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d( - polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); -} - -// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper -// computations for the intermediate steps as assertions. -TEST(IoUTest, PentagonRectangleIntersection) -{ - std::list polygon1{ - {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - const auto intersection = - autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - const auto expected_intersection_area = - autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); - ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. - - const auto expected_union_area = - autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + - autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - - expected_intersection_area; - ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. - - const auto computed_iou = - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); - - EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); -} - -// IoU of two non-intersecting rectangles. -TEST(IoUTest, NoIntersection) -{ - std::list polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - EXPECT_FLOAT_EQ( - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); -} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp deleted file mode 100644 index 266ab123f5203..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::Interval; -using autoware::common::geometry::Interval_d; -using autoware::common::geometry::Interval_f; - -namespace -{ -const auto Inf = std::numeric_limits::infinity(); -const auto Min = std::numeric_limits::lowest(); -const auto Max = std::numeric_limits::max(); -const auto NaN = std::numeric_limits::quiet_NaN(); -const auto epsilon = 1e-5; -} // namespace - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, AbsEq) -{ - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); - const auto shift = (2.0 * epsilon); - const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); - const auto i_empty = Interval_d(); - - EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IsSubsetEq) -{ - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0))); - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d())); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ClampTo) -{ - const auto i = Interval_d(-1.0, 1.0); - { - const auto val = 0.0; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, val); - } - - { - const auto val = -3.4; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::min(i)); - } - - { - const auto val = 2.7; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::max(i)); - } - - const auto val = 1.0; - const auto empty_interval = Interval_d(); - const auto projected = Interval_d::clamp_to(empty_interval, val); - EXPECT_TRUE(std::isnan(projected)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Comparisons) -{ - { - const auto i1 = Interval_d(0.25, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(-0.25, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Contains) -{ - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::contains(i, 0.0)); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_TRUE(Interval_d::contains(i, 0.0)); - EXPECT_FALSE(Interval_d::contains(i, 2.0)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Empty) -{ - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(0.0, 1.0); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ZeroMeasure) -{ - { - const auto i = Interval_d(0, 1); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(2, 2); - EXPECT_TRUE(Interval_d::zero_measure(i)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IntersectionMeasure) -{ - { - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-0.5, 1.5); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_EQ(Interval_d::min(i), -0.5); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); - } - - { - const auto i1 = Interval_d(-2.0, -1.0); - const auto i2 = Interval_d(1.0, 2.0); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i)); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ConstructionMeasure) -{ - { - const auto i = Interval_d(); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_EQ(Interval_d::min(i), -1.0); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); - } - - { - const auto i = Interval_d(0.0, 0.0); - EXPECT_EQ(Interval_d::min(i), 0.0); - EXPECT_EQ(Interval_d::max(i), 0.0); - EXPECT_FALSE(Interval_d::empty(i)); - EXPECT_EQ(Interval_d::measure(i), 0.0); - } - - { - EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error); - } -} - -//------------------------------------------------------------------------------ diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 49a67d68057d9..daf14ba4147c2 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -12,7 +12,6 @@ autoware_cmake - autoware_auto_geometry autoware_auto_mapping_msgs geometry_msgs image_geometry