refactor(shape_estimation)!: fix namespace and directory structure

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
 )

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 <rclcpp/rclcpp.hpp>
 
@@ -46,7 +45,6 @@
 #include <deque>
 #include <memory>
 #include <vector>
-
 namespace autoware::detection_by_tracker
 {
 class Debugger

perception/detection_by_tracker/src/detection_by_tracker_node.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -56,24 +56,26 @@
   return output;
 }
 
-boost::optional<ReferenceYawInfo> getReferenceYawInfo(const uint8_t label, const float yaw)
+boost::optional<autoware::shape_estimation::ReferenceYawInfo> 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<ReferenceShapeSizeInfo> getReferenceShapeSizeInfo(
+boost::optional<autoware::shape_estimation::ReferenceShapeSizeInfo> 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 @@
 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 @@
   // set maximum search setting for merger/divider
   setMaxSearchRange();
 
-  shape_estimator_ = std::make_shared<ShapeEstimator>(true, true);
+  shape_estimator_ = std::make_shared<autoware::shape_estimation::ShapeEstimator>(true, true);
   cluster_ = std::make_shared<euclidean_cluster::VoxelGridBasedEuclideanCluster>(
     false, 10, 10000, 0.7, 0.3, 0);
   debugger_ = std::make_shared<Debugger>(this);
@@ -468,7 +406,6 @@
     out_objects.feature_objects.push_back(feature_object);
   }
 }
-
 }  // namespace autoware::detection_by_tracker
 
 #include <rclcpp_components/register_node_macro.hpp>

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 <rclcpp/rclcpp.hpp>
@@ -52,19 +53,6 @@
 namespace autoware::detection_by_tracker
 {
 
-class TrackerHandler
-{
-private:
-  std::deque<autoware_perception_msgs::msg::TrackedObjects> 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<ShapeEstimator> shape_estimator_;
+  std::shared_ptr<autoware::shape_estimation::ShapeEstimator> shape_estimator_;
   std::shared_ptr<euclidean_cluster::EuclideanClusterInterface> cluster_;
   std::shared_ptr<Debugger> debugger_;
   std::map<uint8_t, int> max_search_distance_for_merger_;
   std::map<uint8_t, int> max_search_distance_for_divider_;
 
-  utils::TrackerIgnoreLabel tracker_ignore_;
+  detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_;
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> 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_

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 <tf2/utils.h>
+
+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

perception/detection_by_tracker/src/utils/utils.cpp → perception/detection_by_tracker/src/tracker/tracker_handler.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// 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.
@@ -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 <autoware_perception_msgs/msg/object_classification.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
+
+#include <deque>
 
 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<autoware_perception_msgs::msg::TrackedObjects> 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_