diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 112187551d92..c019b2be7758 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -17,21 +17,15 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) include_directories( - include SYSTEM ${EIGEN3_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -# Generate exe file -set(DETECTION_BY_TRACKER_SRC - src/detection_by_tracker_node.cpp - src/utils/utils.cpp -) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${DETECTION_BY_TRACKER_SRC} + src/detection_by_tracker_node.cpp + src/tracker/tracker_handler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -43,9 +37,7 @@ rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::detection_by_tracker::DetectionByTracker" EXECUTABLE detection_by_tracker_node ) - -ament_auto_package( - INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE launch config ) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 242056c3ca31..56f37fb10043 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -20,7 +20,6 @@ #include "euclidean_cluster/euclidean_cluster.hpp" #include "euclidean_cluster/utils.hpp" #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" #include @@ -46,7 +45,6 @@ #include #include #include - namespace autoware::detection_by_tracker { class Debugger diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c41d6002d926..c22d015aa11a 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -56,24 +56,26 @@ autoware_perception_msgs::msg::Shape extendShape( return output; } -boost::optional getReferenceYawInfo(const uint8_t label, const float yaw) +boost::optional getReferenceYawInfo( + const uint8_t label, const float yaw) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; + return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; } else { return boost::none; } } -boost::optional getReferenceShapeSizeInfo( +boost::optional getReferenceShapeSizeInfo( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceShapeSizeInfo{shape, ReferenceShapeSizeInfo::Mode::Min}; + return autoware::shape_estimation::ReferenceShapeSizeInfo{ + shape, autoware::shape_estimation::ReferenceShapeSizeInfo::Mode::Min}; } else { return boost::none; } @@ -83,70 +85,6 @@ boost::optional getReferenceShapeSizeInfo( namespace autoware::detection_by_tracker { -void TrackerHandler::onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) -{ - constexpr size_t max_buffer_size = 10; - - // Add tracked objects to buffer - objects_buffer_.push_front(*msg); - - // Remove old data - while (max_buffer_size < objects_buffer_.size()) { - objects_buffer_.pop_back(); - } -} - -bool TrackerHandler::estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) -{ - if (objects_buffer_.empty()) { - return false; - } - - // Get the objects closest to the target time. - const auto target_objects_iter = std::min_element( - objects_buffer_.cbegin(), objects_buffer_.cend(), - [&time]( - autoware_perception_msgs::msg::TrackedObjects first, - autoware_perception_msgs::msg::TrackedObjects second) { - return std::fabs((time - first.header.stamp).seconds()) < - std::fabs((time - second.header.stamp).seconds()); - }); - - // Estimate the pose of the object at the target time - const auto dt = time - target_objects_iter->header.stamp; - output.header.frame_id = target_objects_iter->header.frame_id; - output.header.stamp = time; - for (const auto & object : target_objects_iter->objects) { - const auto & pose_with_covariance = object.kinematics.pose_with_covariance; - const auto & x = pose_with_covariance.pose.position.x; - const auto & y = pose_with_covariance.pose.position.y; - const auto & z = pose_with_covariance.pose.position.z; - const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); - const auto & twist = object.kinematics.twist_with_covariance.twist; - const float & vx = twist.linear.x; - const float & wz = twist.angular.z; - - // build output - autoware_perception_msgs::msg::TrackedObject estimated_object; - estimated_object.object_id = object.object_id; - estimated_object.existence_probability = object.existence_probability; - estimated_object.classification = object.classification; - estimated_object.shape = object.shape; - estimated_object.kinematics.pose_with_covariance.pose.position.x = - x + vx * std::cos(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.y = - y + vx * std::sin(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); - estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_hat); - output.objects.push_back(estimated_object); - } - return true; -} - DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("detection_by_tracker", node_options), tf_buffer_(this->get_clock()), @@ -176,7 +114,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) // set maximum search setting for merger/divider setMaxSearchRange(); - shape_estimator_ = std::make_shared(true, true); + shape_estimator_ = std::make_shared(true, true); cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); @@ -468,7 +406,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( out_objects.feature_objects.push_back(feature_object); } } - } // namespace autoware::detection_by_tracker #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 4e58d53fa71a..095708fb4b51 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,13 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" #include "euclidean_cluster/euclidean_cluster.hpp" #include "euclidean_cluster/utils.hpp" #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" #include @@ -52,19 +53,6 @@ namespace autoware::detection_by_tracker { -class TrackerHandler -{ -private: - std::deque objects_buffer_; - -public: - TrackerHandler() = default; - void onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); - bool estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); -}; - class DetectionByTracker : public rclcpp::Node { public: @@ -80,13 +68,13 @@ class DetectionByTracker : public rclcpp::Node tf2_ros::TransformListener tf_listener_; TrackerHandler tracker_handler_; - std::shared_ptr shape_estimator_; + std::shared_ptr shape_estimator_; std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; std::unique_ptr published_time_publisher_; @@ -112,7 +100,6 @@ class DetectionByTracker : public rclcpp::Node autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); }; - } // namespace autoware::detection_by_tracker #endif // DETECTION_BY_TRACKER_NODE_HPP_ diff --git a/perception/detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/detection_by_tracker/src/tracker/tracker_handler.cpp new file mode 100644 index 000000000000..a099c8b1ba55 --- /dev/null +++ b/perception/detection_by_tracker/src/tracker/tracker_handler.cpp @@ -0,0 +1,88 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracker_handler.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" + +#include + +namespace autoware::detection_by_tracker +{ + +void TrackerHandler::onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) +{ + constexpr size_t max_buffer_size = 10; + + // Add tracked objects to buffer + objects_buffer_.push_front(*msg); + + // Remove old data + while (max_buffer_size < objects_buffer_.size()) { + objects_buffer_.pop_back(); + } +} + +bool TrackerHandler::estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) +{ + if (objects_buffer_.empty()) { + return false; + } + + // Get the objects closest to the target time. + const auto target_objects_iter = std::min_element( + objects_buffer_.cbegin(), objects_buffer_.cend(), + [&time]( + autoware_perception_msgs::msg::TrackedObjects first, + autoware_perception_msgs::msg::TrackedObjects second) { + return std::fabs((time - first.header.stamp).seconds()) < + std::fabs((time - second.header.stamp).seconds()); + }); + + // Estimate the pose of the object at the target time + const auto dt = time - target_objects_iter->header.stamp; + output.header.frame_id = target_objects_iter->header.frame_id; + output.header.stamp = time; + for (const auto & object : target_objects_iter->objects) { + const auto & pose_with_covariance = object.kinematics.pose_with_covariance; + const auto & x = pose_with_covariance.pose.position.x; + const auto & y = pose_with_covariance.pose.position.y; + const auto & z = pose_with_covariance.pose.position.z; + const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); + const auto & twist = object.kinematics.twist_with_covariance.twist; + const float & vx = twist.linear.x; + const float & wz = twist.angular.z; + + // build output + autoware_perception_msgs::msg::TrackedObject estimated_object; + estimated_object.object_id = object.object_id; + estimated_object.existence_probability = object.existence_probability; + estimated_object.classification = object.classification; + estimated_object.shape = object.shape; + estimated_object.kinematics.pose_with_covariance.pose.position.x = + x + vx * std::cos(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.y = + y + vx * std::sin(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.z = z; + const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); + estimated_object.kinematics.pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(yaw_hat); + output.objects.push_back(estimated_object); + } + return true; +} +} // namespace autoware::detection_by_tracker diff --git a/perception/detection_by_tracker/src/utils/utils.cpp b/perception/detection_by_tracker/src/tracker/tracker_handler.hpp similarity index 50% rename from perception/detection_by_tracker/src/utils/utils.cpp rename to perception/detection_by_tracker/src/tracker/tracker_handler.hpp index b269d703be7d..13aa00aaf0c4 100644 --- a/perception/detection_by_tracker/src/utils/utils.cpp +++ b/perception/detection_by_tracker/src/tracker/tracker_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils.hpp" +#ifndef TRACKER__TRACKER_HANDLER_HPP_ +#define TRACKER__TRACKER_HANDLER_HPP_ -#include +#include + +#include "autoware_perception_msgs/msg/tracked_objects.hpp" + +#include namespace autoware::detection_by_tracker { -namespace utils -{ -using Label = autoware_perception_msgs::msg::ObjectClassification; -bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const +class TrackerHandler { - return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || - (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || - (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || - (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); -} -} // namespace utils +private: + std::deque objects_buffer_; + +public: + TrackerHandler() = default; + void onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); + bool estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); +}; + } // namespace autoware::detection_by_tracker + +#endif // TRACKER__TRACKER_HANDLER_HPP_ diff --git a/perception/detection_by_tracker/src/utils/utils.hpp b/perception/detection_by_tracker/src/utils/utils.hpp index b7041124fa93..cc29164e4aad 100644 --- a/perception/detection_by_tracker/src/utils/utils.hpp +++ b/perception/detection_by_tracker/src/utils/utils.hpp @@ -15,12 +15,16 @@ #ifndef UTILS__UTILS_HPP_ #define UTILS__UTILS_HPP_ +#include "autoware_perception_msgs/msg/object_classification.hpp" + #include namespace autoware::detection_by_tracker { namespace utils { +using Label = autoware_perception_msgs::msg::ObjectClassification; + struct TrackerIgnoreLabel { bool UNKNOWN; @@ -31,8 +35,15 @@ struct TrackerIgnoreLabel bool MOTORCYCLE; bool BICYCLE; bool PEDESTRIAN; - bool isIgnore(const uint8_t label) const; -}; // struct TrackerIgnoreLabel + bool isIgnore(const uint8_t label) const + { + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); + } +}; + } // namespace utils } // namespace autoware::detection_by_tracker diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 527c565b91e0..21517f8c0f76 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -16,7 +16,7 @@ set(SHAPE_ESTIMATION_DEPENDENCIES Eigen3 ) -ament_auto_add_library(shape_estimation_lib SHARED +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/shape_estimator.cpp lib/model/bounding_box.cpp lib/model/convex_hull.cpp @@ -27,23 +27,23 @@ ament_auto_add_library(shape_estimation_lib SHARED lib/filter/trailer_filter.cpp lib/filter/no_filter.cpp lib/filter/utils.cpp - lib/corrector/no_corrector.cpp lib/corrector/utils.cpp + lib/corrector/no_corrector.cpp ) -ament_target_dependencies(shape_estimation_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME}_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_include_directories(shape_estimation_lib +target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC "${PCL_INCLUDE_DIRS}" "${EIGEN3_INCLUDE_DIR}" ) -ament_auto_add_library(shape_estimation_node SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/shape_estimation_node.cpp ) -target_include_directories(shape_estimation_node +target_include_directories(${PROJECT_NAME} PUBLIC $ $ @@ -51,15 +51,15 @@ target_include_directories(shape_estimation_node ${CMAKE_CURRENT_SOURCE_DIR}/src ) -ament_target_dependencies(shape_estimation_node ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME} ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_link_libraries(shape_estimation_node +target_link_libraries(${PROJECT_NAME} shape_estimation_lib ) -rclcpp_components_register_node(shape_estimation_node - PLUGIN "ShapeEstimationNode" - EXECUTABLE shape_estimation +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::shape_estimation::ShapeEstimationNode" + EXECUTABLE shape_estimation_node ) ament_auto_package(INSTALL_TO_SHARE @@ -79,6 +79,6 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) target_link_libraries(test_shape_estimation - shape_estimation_node + ${PROJECT_NAME} ) endif() diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index 206e8d422c72..9615b314f62c 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,7 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} +{{ json_to_markdown("perception/autoware/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp index ec1a2b8a4297..f8dc489a563b 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class BicycleCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class BicycleCorrector : public VehicleCorrector ~BicycleCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp index 4ef1f00122ca..d7386ace5b36 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace corrector +{ class BusCorrector : public VehicleCorrector { public: @@ -36,4 +39,7 @@ class BusCorrector : public VehicleCorrector ~BusCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp index d4d33599c773..7292704d0f6d 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class CarCorrector : public VehicleCorrector { public: @@ -36,4 +41,7 @@ class CarCorrector : public VehicleCorrector ~CarCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp new file mode 100644 index 000000000000..dd4c4f68d3f4 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp @@ -0,0 +1,27 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ + +#include "autoware/shape_estimation/corrector/bicycle_corrector.hpp" +#include "autoware/shape_estimation/corrector/bus_corrector.hpp" +#include "autoware/shape_estimation/corrector/car_corrector.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/no_corrector.hpp" +#include "autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp" +#include "autoware/shape_estimation/corrector/trailer_corrector.hpp" +#include "autoware/shape_estimation/corrector/truck_corrector.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp index 1ce1273f042b..1dcf85b46887 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ #include #include #include - +namespace autoware::shape_estimation +{ +namespace corrector +{ class ShapeEstimationCorrectorInterface { public: @@ -31,4 +34,7 @@ class ShapeEstimationCorrectorInterface autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp index 1303f6f85919..e3c33100861a 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class NoCorrector : public ShapeEstimationCorrectorInterface { public: @@ -26,7 +31,14 @@ class NoCorrector : public ShapeEstimationCorrectorInterface ~NoCorrector() {} bool correct( - autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp index 0d1740dc30d6..a05027a3348b 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInterface { ReferenceShapeSizeInfo ref_shape_size_info_; @@ -38,4 +43,6 @@ class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInter } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp index 6dd885353c78..afd0f1f7c451 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + // Generally speaking, trailer would be much larger than bus and truck. // But currently we do not make large differences among bus/truck/trailer // because current our vehicle classification is not reliable enough. @@ -40,4 +45,7 @@ class TrailerCorrector : public VehicleCorrector ~TrailerCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp index 3b2475286fa6..8a641e84176a 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class TruckCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class TruckCorrector : public VehicleCorrector ~TruckCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp similarity index 82% rename from perception/shape_estimation/include/shape_estimation/corrector/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp index 951f3d16aa27..a16cb146ca4a 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include +namespace autoware::shape_estimation +{ + namespace corrector_utils { struct CorrectionBBParameters @@ -46,4 +49,6 @@ bool correctWithReferenceYaw( } // namespace corrector_utils -#endif // SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp index 1b38f1562066..e1b292eb5318 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class VehicleCorrector : public ShapeEstimationCorrectorInterface { protected: @@ -47,4 +52,7 @@ class VehicleCorrector : public ShapeEstimationCorrectorInterface void setParams(const corrector_utils::CorrectionBBParameters & params) { params_ = params; } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp index 301921b00d43..54d45498a3d4 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class BusFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class BusFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp index 79104823bb7a..8ff79f901d8a 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class CarFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class CarFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose) override; }; -#endif // SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp new file mode 100644 index 000000000000..5b7bad4f5720 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp @@ -0,0 +1,25 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ + +#include "autoware/shape_estimation/filter/bus_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/no_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp index 67dfe27eb93b..cba7715314a9 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ #include #include #include +namespace autoware::shape_estimation +{ +namespace filter +{ + class ShapeEstimationFilterInterface { public: @@ -31,4 +36,7 @@ class ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp index d007e6c61c96..f05c0e9affa0 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ class NoFilter : public ShapeEstimationFilterInterface { @@ -25,8 +30,14 @@ class NoFilter : public ShapeEstimationFilterInterface ~NoFilter() = default; bool filter( - const autoware_perception_msgs::msg::Shape & shape, - const geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] const geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp index 319d3e25c77c..21394938a802 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class TrailerFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +33,7 @@ class TrailerFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp index e67da1c31115..a941ec4fd793 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace filter +{ class TruckFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +32,7 @@ class TruckFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/filter/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp index 8d487b8d42a9..7f56664e0a57 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__UTILS_HPP_ -#define SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ #include +namespace autoware::shape_estimation +{ namespace utils { @@ -24,4 +26,6 @@ bool filterVehicleBoundingBox( const float max_length); } // namespace utils -#endif // SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp similarity index 80% rename from perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp index 2ac179d03062..c9d6889c21de 100644 --- a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#define SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#include "shape_estimation/model/model_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include +namespace autoware::shape_estimation +{ +namespace model +{ + class BoundingBoxShapeModel : public ShapeEstimationModelInterface { private: @@ -47,4 +52,7 @@ class BoundingBoxShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp similarity index 71% rename from perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp index 2748d4a53a80..c3ffa297833f 100644 --- a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#define SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class ConvexHullShapeModel : public ShapeEstimationModelInterface { @@ -30,4 +35,7 @@ class ConvexHullShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/model/cylinder.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp index ccd28afa3173..6623934cdcc2 100644 --- a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#define SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class CylinderShapeModel : public ShapeEstimationModelInterface { @@ -33,4 +38,7 @@ class CylinderShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp similarity index 60% rename from perception/shape_estimation/include/shape_estimation/model/model.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp index 65080ac88407..382812de6f18 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#include "shape_estimation/model/bounding_box.hpp" -#include "shape_estimation/model/convex_hull.hpp" -#include "shape_estimation/model/cylinder.hpp" -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/bounding_box.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" -#endif // SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp similarity index 78% rename from perception/shape_estimation/include/shape_estimation/model/model_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp index f5324196f69b..380e88704baa 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #include #include @@ -24,6 +24,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + class ShapeEstimationModelInterface { public: @@ -37,4 +42,7 @@ class ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) = 0; }; -#endif // SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp similarity index 90% rename from perception/shape_estimation/include/shape_estimation/shape_estimator.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp index 7cdab6f86b58..9759c7c77855 100644 --- a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ -#define SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + struct ReferenceYawInfo { float yaw; @@ -75,5 +78,6 @@ class ShapeEstimator const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output); }; +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp deleted file mode 100644 index e4efc17181e5..000000000000 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ - -#include "shape_estimation/corrector/bicycle_corrector.hpp" -#include "shape_estimation/corrector/bus_corrector.hpp" -#include "shape_estimation/corrector/car_corrector.hpp" -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/corrector/no_corrector.hpp" -#include "shape_estimation/corrector/reference_shape_size_corrector.hpp" -#include "shape_estimation/corrector/trailer_corrector.hpp" -#include "shape_estimation/corrector/truck_corrector.hpp" - -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp deleted file mode 100644 index b205cbd6791b..000000000000 --- a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SHAPE_ESTIMATION__FILTER__FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_HPP_ - -#include "shape_estimation/filter/bus_filter.hpp" -#include "shape_estimation/filter/car_filter.hpp" -#include "shape_estimation/filter/filter_interface.hpp" -#include "shape_estimation/filter/no_filter.hpp" -#include "shape_estimation/filter/trailer_filter.hpp" -#include "shape_estimation/filter/truck_filter.hpp" - -#endif // SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index f13b5d1e5f27..88cea81412d9 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/shape_estimation/lib/corrector/no_corrector.cpp b/perception/shape_estimation/lib/corrector/no_corrector.cpp index e18de4acefcd..b37ee289871d 100644 --- a/perception/shape_estimation/lib/corrector/no_corrector.cpp +++ b/perception/shape_estimation/lib/corrector/no_corrector.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/no_corrector.hpp" - -bool NoCorrector::correct( - [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape_output, - [[maybe_unused]] geometry_msgs::msg::Pose & pose_output) -{ - return true; -} +#include "autoware/shape_estimation/corrector/no_corrector.hpp" diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index 1332647b0f1f..a18e09d5b8a0 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/utils.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/corrector/utils.hpp" -#include +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -28,12 +30,14 @@ #include #endif +#include +#include + #include #include -#define EIGEN_MPL2_ONLY -#include -#include +namespace autoware::shape_estimation +{ namespace corrector_utils { @@ -398,3 +402,5 @@ bool correctWithReferenceYawAndShapeSize( return true; } } // namespace corrector_utils + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/bus_filter.cpp b/perception/shape_estimation/lib/filter/bus_filter.cpp index 536545813237..a301ce716cca 100644 --- a/perception/shape_estimation/lib/filter/bus_filter.cpp +++ b/perception/shape_estimation/lib/filter/bus_filter.cpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/bus_filter.hpp" - +#include "autoware/shape_estimation/filter/bus_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool BusFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +26,6 @@ bool BusFilter::filter( constexpr float max_length = 17.0; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/car_filter.cpp b/perception/shape_estimation/lib/filter/car_filter.cpp index e12b9c211a24..ab7a8d69d109 100644 --- a/perception/shape_estimation/lib/filter/car_filter.cpp +++ b/perception/shape_estimation/lib/filter/car_filter.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool CarFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +27,6 @@ bool CarFilter::filter( constexpr float max_length = 5.8; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/no_filter.cpp b/perception/shape_estimation/lib/filter/no_filter.cpp index 564de63461c4..22690be2ed76 100644 --- a/perception/shape_estimation/lib/filter/no_filter.cpp +++ b/perception/shape_estimation/lib/filter/no_filter.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/no_filter.hpp" - -bool NoFilter::filter( - [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, - [[maybe_unused]] const geometry_msgs::msg::Pose & pose) -{ - return true; -} +#include "autoware/shape_estimation/filter/no_filter.hpp" diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/shape_estimation/lib/filter/trailer_filter.cpp index 5087350f4d60..378ce085aa88 100644 --- a/perception/shape_estimation/lib/filter/trailer_filter.cpp +++ b/perception/shape_estimation/lib/filter/trailer_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TrailerFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TrailerFilter::filter( constexpr float max_length = 25.0; // maximum Full-TRAILER size in JAPAN(normally upto 18m) return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index ad733bb58c97..4afcccaf5534 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/truck_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TruckFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TruckFilter::filter( constexpr float max_length = 18.0; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/utils.cpp b/perception/shape_estimation/lib/filter/utils.cpp index 9412be4e99a9..616eb5e22769 100644 --- a/perception/shape_estimation/lib/filter/utils.cpp +++ b/perception/shape_estimation/lib/filter/utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/utils.hpp" +#include "autoware/shape_estimation/filter/utils.hpp" +namespace autoware::shape_estimation +{ namespace utils { @@ -43,4 +45,6 @@ bool filterVehicleBoundingBox( } return true; } + } // namespace utils +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index a6693fa56d55..df751be27d8c 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/bounding_box.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/model/bounding_box.hpp" #include #include #include -#include +#include "autoware_perception_msgs/msg/shape.hpp" #include @@ -33,14 +35,17 @@ #include #endif +#include + #include #include #include #include -#define EIGEN_MPL2_ONLY - -#include +namespace autoware::shape_estimation +{ +namespace model +{ constexpr float epsilon = 0.001; @@ -252,3 +257,6 @@ float BoundingBoxShapeModel::boostOptimize( float theta_star = min.first; return theta_star; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/convex_hull.cpp b/perception/shape_estimation/lib/model/convex_hull.cpp index d29fb6aa90a9..3c68634aaff4 100644 --- a/perception/shape_estimation/lib/model/convex_hull.cpp +++ b/perception/shape_estimation/lib/model/convex_hull.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" #include #include @@ -28,6 +28,11 @@ #include #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool ConvexHullShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -94,3 +99,6 @@ bool ConvexHullShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/cylinder.cpp b/perception/shape_estimation/lib/model/cylinder.cpp index 9b3f58854df9..3f3139222a2b 100644 --- a/perception/shape_estimation/lib/model/cylinder.cpp +++ b/perception/shape_estimation/lib/model/cylinder.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" #include #include @@ -26,6 +26,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool CylinderShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -63,3 +68,6 @@ bool CylinderShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 85bc3cd8bd9d..34eed5e64136 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer) @@ -75,15 +78,15 @@ bool ShapeEstimator::estimateOriginalShapeAndPose( autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) { // estimate shape - std::unique_ptr model_ptr; + std::unique_ptr model_ptr; if ( label == Label::CAR || label == Label::TRUCK || label == Label::BUS || label == Label::TRAILER || label == Label::MOTORCYCLE || label == Label::BICYCLE) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); + model_ptr.reset(new model::BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); } else if (label == Label::PEDESTRIAN) { - model_ptr.reset(new CylinderShapeModel()); + model_ptr.reset(new model::CylinderShapeModel()); } else { - model_ptr.reset(new ConvexHullShapeModel()); + model_ptr.reset(new model::ConvexHullShapeModel()); } return model_ptr->estimate(cluster, shape_output, pose_output); @@ -93,17 +96,17 @@ bool ShapeEstimator::applyFilter( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) { - std::unique_ptr filter_ptr; + std::unique_ptr filter_ptr; if (label == Label::CAR) { - filter_ptr.reset(new CarFilter); + filter_ptr.reset(new filter::CarFilter); } else if (label == Label::BUS) { - filter_ptr.reset(new BusFilter); + filter_ptr.reset(new filter::BusFilter); } else if (label == Label::TRUCK) { - filter_ptr.reset(new TruckFilter); + filter_ptr.reset(new filter::TruckFilter); } else if (label == Label::TRAILER) { - filter_ptr.reset(new TrailerFilter); + filter_ptr.reset(new filter::TrailerFilter); } else { - filter_ptr.reset(new NoFilter); + filter_ptr.reset(new filter::NoFilter); } return filter_ptr->filter(shape, pose); @@ -114,23 +117,26 @@ bool ShapeEstimator::applyCorrector( const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) { - std::unique_ptr corrector_ptr; + std::unique_ptr corrector_ptr; if (ref_shape_size_info && use_reference_yaw) { - corrector_ptr.reset(new ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); + corrector_ptr.reset( + new corrector::ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); } else if (label == Label::CAR) { - corrector_ptr.reset(new CarCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::CarCorrector(use_reference_yaw)); } else if (label == Label::BUS) { - corrector_ptr.reset(new BusCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BusCorrector(use_reference_yaw)); } else if (label == Label::TRUCK) { - corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { - corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TrailerCorrector(use_reference_yaw)); } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { - corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BicycleCorrector(use_reference_yaw)); } else { - corrector_ptr.reset(new NoCorrector); + corrector_ptr.reset(new corrector::NoCorrector); } return corrector_ptr->correct(shape, pose); } + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/shape_estimation_node.cpp similarity index 93% rename from perception/shape_estimation/src/node.cpp rename to perception/shape_estimation/src/shape_estimation_node.cpp index 5719cd97b3ed..4a373b15d580 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/shape_estimation_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "shape_estimation_node.hpp" -#include -#include +#include "autoware/shape_estimation/shape_estimator.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include @@ -32,6 +32,9 @@ #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_options) @@ -143,6 +146,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } +} // namespace autoware::shape_estimation #include -RCLCPP_COMPONENTS_REGISTER_NODE(ShapeEstimationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::shape_estimation::ShapeEstimationNode) diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/shape_estimation_node.hpp similarity index 88% rename from perception/shape_estimation/src/node.hpp rename to perception/shape_estimation/src/shape_estimation_node.hpp index 807f12e39b02..d9f944346611 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/shape_estimation_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef SHAPE_ESTIMATION_NODE_HPP_ +#define SHAPE_ESTIMATION_NODE_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + using autoware_perception_msgs::msg::DetectedObjects; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; class ShapeEstimationNode : public rclcpp::Node @@ -51,5 +54,6 @@ class ShapeEstimationNode : public rclcpp::Node public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); }; +} // namespace autoware::shape_estimation -#endif // NODE_HPP_ +#endif // SHAPE_ESTIMATION_NODE_HPP_ diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp index 0c41e04e5ddd..2e6dcc62d869 100644 --- a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp +++ b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -70,7 +70,7 @@ pcl::PointCloud createLShapeCluster( TEST(BoundingBoxShapeModel, test_estimateShape) { // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel(); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel(); // Generate cluster const double length = 4.0; @@ -117,12 +117,12 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) pcl::PointCloud cluster = createLShapeCluster(length, width, height, yaw, offset_x, offset_y); - const auto ref_yaw_info = - ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const auto ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const bool use_boost_bbox_optimizer = true; // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = - BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel( + ref_yaw_info, use_boost_bbox_optimizer); // Generate shape and pose output autoware_perception_msgs::msg::Shape shape_output; @@ -152,7 +152,7 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) TEST(CylinderShapeModel, test_estimateShape) { // Generate CylinderShapeModel - CylinderShapeModel cylinder_shape_model = CylinderShapeModel(); + auto cylinder_shape_model = autoware::shape_estimation::model::CylinderShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0); @@ -169,7 +169,7 @@ TEST(CylinderShapeModel, test_estimateShape) TEST(ConvexHullShapeModel, test_estimateShape) { // Generate ConvexHullShapeModel - ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel(); + auto convex_hull_shape_model = autoware::shape_estimation::model::ConvexHullShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0); @@ -200,14 +200,16 @@ TEST(ShapeEstimator, test_estimateShapeAndPose) const bool use_corrector = true; const bool use_filter = true; const bool use_boost_bbox_optimizer = true; - ShapeEstimator shape_estimator = - ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); + auto shape_estimator = + autoware::shape_estimation::ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); // Generate ref_yaw_info - boost::optional ref_yaw_info = boost::none; - boost::optional ref_shape_size_info = boost::none; + boost::optional ref_yaw_info = boost::none; + boost::optional ref_shape_size_info = + boost::none; - ref_yaw_info = ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const auto label = autoware_perception_msgs::msg::ObjectClassification::CAR; // Generate shape and pose output