Skip to content

Commit 26e87ea

Browse files
technolojinYoshiRi
technolojin
and
YoshiRi
authored
refactor(shape_estimation)!: fix namespace and directory structure (autowarefoundation#7844)
* chore: Update shape_estimation library and node names refactor: relocate headers refactor: add namespace refactor: update shape_estimation namespace on the user side Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: update namespace of the node refactor: update namespace of the node refactor: update namespace and directory structure for detection_by_tracker node Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: update namespace and directory structure for detection_by_tracker node Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Update perception/detection_by_tracker/src/tracker/tracker_handler.hpp Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com> Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
1 parent 5ebaed7 commit 26e87ea

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+609
-362
lines changed

‎perception/detection_by_tracker/CMakeLists.txt

+3-11
Original file line numberDiff line numberDiff line change
@@ -17,21 +17,15 @@ find_package(eigen3_cmake_module REQUIRED)
1717
find_package(Eigen3 REQUIRED)
1818

1919
include_directories(
20-
include
2120
SYSTEM
2221
${EIGEN3_INCLUDE_DIRS}
2322
${PCL_COMMON_INCLUDE_DIRS}
2423
${PCL_INCLUDE_DIRS}
2524
)
2625

27-
# Generate exe file
28-
set(DETECTION_BY_TRACKER_SRC
29-
src/detection_by_tracker_node.cpp
30-
src/utils/utils.cpp
31-
)
32-
3326
ament_auto_add_library(${PROJECT_NAME} SHARED
34-
${DETECTION_BY_TRACKER_SRC}
27+
src/detection_by_tracker_node.cpp
28+
src/tracker/tracker_handler.cpp
3529
)
3630

3731
target_link_libraries(${PROJECT_NAME}
@@ -43,9 +37,7 @@ rclcpp_components_register_node(${PROJECT_NAME}
4337
PLUGIN "autoware::detection_by_tracker::DetectionByTracker"
4438
EXECUTABLE detection_by_tracker_node
4539
)
46-
47-
ament_auto_package(
48-
INSTALL_TO_SHARE
40+
ament_auto_package(INSTALL_TO_SHARE
4941
launch
5042
config
5143
)

‎perception/detection_by_tracker/src/debugger/debugger.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
#include "euclidean_cluster/euclidean_cluster.hpp"
2121
#include "euclidean_cluster/utils.hpp"
2222
#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
23-
#include "shape_estimation/shape_estimator.hpp"
2423

2524
#include <rclcpp/rclcpp.hpp>
2625

@@ -46,7 +45,6 @@
4645
#include <deque>
4746
#include <memory>
4847
#include <vector>
49-
5048
namespace autoware::detection_by_tracker
5149
{
5250
class Debugger

‎perception/detection_by_tracker/src/detection_by_tracker_node.cpp

+7-70
Original file line numberDiff line numberDiff line change
@@ -56,24 +56,26 @@ autoware_perception_msgs::msg::Shape extendShape(
5656
return output;
5757
}
5858

59-
boost::optional<ReferenceYawInfo> getReferenceYawInfo(const uint8_t label, const float yaw)
59+
boost::optional<autoware::shape_estimation::ReferenceYawInfo> getReferenceYawInfo(
60+
const uint8_t label, const float yaw)
6061
{
6162
const bool is_vehicle =
6263
Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label;
6364
if (is_vehicle) {
64-
return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)};
65+
return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)};
6566
} else {
6667
return boost::none;
6768
}
6869
}
6970

70-
boost::optional<ReferenceShapeSizeInfo> getReferenceShapeSizeInfo(
71+
boost::optional<autoware::shape_estimation::ReferenceShapeSizeInfo> getReferenceShapeSizeInfo(
7172
const uint8_t label, const autoware_perception_msgs::msg::Shape & shape)
7273
{
7374
const bool is_vehicle =
7475
Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label;
7576
if (is_vehicle) {
76-
return ReferenceShapeSizeInfo{shape, ReferenceShapeSizeInfo::Mode::Min};
77+
return autoware::shape_estimation::ReferenceShapeSizeInfo{
78+
shape, autoware::shape_estimation::ReferenceShapeSizeInfo::Mode::Min};
7779
} else {
7880
return boost::none;
7981
}
@@ -83,70 +85,6 @@ boost::optional<ReferenceShapeSizeInfo> getReferenceShapeSizeInfo(
8385
namespace autoware::detection_by_tracker
8486
{
8587

86-
void TrackerHandler::onTrackedObjects(
87-
const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg)
88-
{
89-
constexpr size_t max_buffer_size = 10;
90-
91-
// Add tracked objects to buffer
92-
objects_buffer_.push_front(*msg);
93-
94-
// Remove old data
95-
while (max_buffer_size < objects_buffer_.size()) {
96-
objects_buffer_.pop_back();
97-
}
98-
}
99-
100-
bool TrackerHandler::estimateTrackedObjects(
101-
const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output)
102-
{
103-
if (objects_buffer_.empty()) {
104-
return false;
105-
}
106-
107-
// Get the objects closest to the target time.
108-
const auto target_objects_iter = std::min_element(
109-
objects_buffer_.cbegin(), objects_buffer_.cend(),
110-
[&time](
111-
autoware_perception_msgs::msg::TrackedObjects first,
112-
autoware_perception_msgs::msg::TrackedObjects second) {
113-
return std::fabs((time - first.header.stamp).seconds()) <
114-
std::fabs((time - second.header.stamp).seconds());
115-
});
116-
117-
// Estimate the pose of the object at the target time
118-
const auto dt = time - target_objects_iter->header.stamp;
119-
output.header.frame_id = target_objects_iter->header.frame_id;
120-
output.header.stamp = time;
121-
for (const auto & object : target_objects_iter->objects) {
122-
const auto & pose_with_covariance = object.kinematics.pose_with_covariance;
123-
const auto & x = pose_with_covariance.pose.position.x;
124-
const auto & y = pose_with_covariance.pose.position.y;
125-
const auto & z = pose_with_covariance.pose.position.z;
126-
const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation);
127-
const auto & twist = object.kinematics.twist_with_covariance.twist;
128-
const float & vx = twist.linear.x;
129-
const float & wz = twist.angular.z;
130-
131-
// build output
132-
autoware_perception_msgs::msg::TrackedObject estimated_object;
133-
estimated_object.object_id = object.object_id;
134-
estimated_object.existence_probability = object.existence_probability;
135-
estimated_object.classification = object.classification;
136-
estimated_object.shape = object.shape;
137-
estimated_object.kinematics.pose_with_covariance.pose.position.x =
138-
x + vx * std::cos(yaw) * dt.seconds();
139-
estimated_object.kinematics.pose_with_covariance.pose.position.y =
140-
y + vx * std::sin(yaw) * dt.seconds();
141-
estimated_object.kinematics.pose_with_covariance.pose.position.z = z;
142-
const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds());
143-
estimated_object.kinematics.pose_with_covariance.pose.orientation =
144-
autoware::universe_utils::createQuaternionFromYaw(yaw_hat);
145-
output.objects.push_back(estimated_object);
146-
}
147-
return true;
148-
}
149-
15088
DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
15189
: rclcpp::Node("detection_by_tracker", node_options),
15290
tf_buffer_(this->get_clock()),
@@ -176,7 +114,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
176114
// set maximum search setting for merger/divider
177115
setMaxSearchRange();
178116

179-
shape_estimator_ = std::make_shared<ShapeEstimator>(true, true);
117+
shape_estimator_ = std::make_shared<autoware::shape_estimation::ShapeEstimator>(true, true);
180118
cluster_ = std::make_shared<euclidean_cluster::VoxelGridBasedEuclideanCluster>(
181119
false, 10, 10000, 0.7, 0.3, 0);
182120
debugger_ = std::make_shared<Debugger>(this);
@@ -468,7 +406,6 @@ void DetectionByTracker::mergeOverSegmentedObjects(
468406
out_objects.feature_objects.push_back(feature_object);
469407
}
470408
}
471-
472409
} // namespace autoware::detection_by_tracker
473410

474411
#include <rclcpp_components/register_node_macro.hpp>

‎perception/detection_by_tracker/src/detection_by_tracker_node.hpp

+4-17
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,13 @@
1515
#ifndef DETECTION_BY_TRACKER_NODE_HPP_
1616
#define DETECTION_BY_TRACKER_NODE_HPP_
1717

18+
#include "autoware/shape_estimation/shape_estimator.hpp"
1819
#include "autoware/universe_utils/ros/published_time_publisher.hpp"
1920
#include "debugger/debugger.hpp"
2021
#include "euclidean_cluster/euclidean_cluster.hpp"
2122
#include "euclidean_cluster/utils.hpp"
2223
#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
23-
#include "shape_estimation/shape_estimator.hpp"
24+
#include "tracker/tracker_handler.hpp"
2425
#include "utils/utils.hpp"
2526

2627
#include <rclcpp/rclcpp.hpp>
@@ -52,19 +53,6 @@
5253
namespace autoware::detection_by_tracker
5354
{
5455

55-
class TrackerHandler
56-
{
57-
private:
58-
std::deque<autoware_perception_msgs::msg::TrackedObjects> objects_buffer_;
59-
60-
public:
61-
TrackerHandler() = default;
62-
void onTrackedObjects(
63-
const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg);
64-
bool estimateTrackedObjects(
65-
const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output);
66-
};
67-
6856
class DetectionByTracker : public rclcpp::Node
6957
{
7058
public:
@@ -80,13 +68,13 @@ class DetectionByTracker : public rclcpp::Node
8068
tf2_ros::TransformListener tf_listener_;
8169

8270
TrackerHandler tracker_handler_;
83-
std::shared_ptr<ShapeEstimator> shape_estimator_;
71+
std::shared_ptr<autoware::shape_estimation::ShapeEstimator> shape_estimator_;
8472
std::shared_ptr<euclidean_cluster::EuclideanClusterInterface> cluster_;
8573
std::shared_ptr<Debugger> debugger_;
8674
std::map<uint8_t, int> max_search_distance_for_merger_;
8775
std::map<uint8_t, int> max_search_distance_for_divider_;
8876

89-
utils::TrackerIgnoreLabel tracker_ignore_;
77+
detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_;
9078

9179
std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
9280

@@ -112,7 +100,6 @@ class DetectionByTracker : public rclcpp::Node
112100
autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects,
113101
tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects);
114102
};
115-
116103
} // namespace autoware::detection_by_tracker
117104

118105
#endif // DETECTION_BY_TRACKER_NODE_HPP_

‎perception/detection_by_tracker/src/tracker/tracker_handler.cpp

+88
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright 2021 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "tracker_handler.hpp"
16+
17+
#include "autoware/universe_utils/geometry/geometry.hpp"
18+
#include "autoware/universe_utils/math/normalization.hpp"
19+
20+
#include <tf2/utils.h>
21+
22+
namespace autoware::detection_by_tracker
23+
{
24+
25+
void TrackerHandler::onTrackedObjects(
26+
const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg)
27+
{
28+
constexpr size_t max_buffer_size = 10;
29+
30+
// Add tracked objects to buffer
31+
objects_buffer_.push_front(*msg);
32+
33+
// Remove old data
34+
while (max_buffer_size < objects_buffer_.size()) {
35+
objects_buffer_.pop_back();
36+
}
37+
}
38+
39+
bool TrackerHandler::estimateTrackedObjects(
40+
const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output)
41+
{
42+
if (objects_buffer_.empty()) {
43+
return false;
44+
}
45+
46+
// Get the objects closest to the target time.
47+
const auto target_objects_iter = std::min_element(
48+
objects_buffer_.cbegin(), objects_buffer_.cend(),
49+
[&time](
50+
autoware_perception_msgs::msg::TrackedObjects first,
51+
autoware_perception_msgs::msg::TrackedObjects second) {
52+
return std::fabs((time - first.header.stamp).seconds()) <
53+
std::fabs((time - second.header.stamp).seconds());
54+
});
55+
56+
// Estimate the pose of the object at the target time
57+
const auto dt = time - target_objects_iter->header.stamp;
58+
output.header.frame_id = target_objects_iter->header.frame_id;
59+
output.header.stamp = time;
60+
for (const auto & object : target_objects_iter->objects) {
61+
const auto & pose_with_covariance = object.kinematics.pose_with_covariance;
62+
const auto & x = pose_with_covariance.pose.position.x;
63+
const auto & y = pose_with_covariance.pose.position.y;
64+
const auto & z = pose_with_covariance.pose.position.z;
65+
const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation);
66+
const auto & twist = object.kinematics.twist_with_covariance.twist;
67+
const float & vx = twist.linear.x;
68+
const float & wz = twist.angular.z;
69+
70+
// build output
71+
autoware_perception_msgs::msg::TrackedObject estimated_object;
72+
estimated_object.object_id = object.object_id;
73+
estimated_object.existence_probability = object.existence_probability;
74+
estimated_object.classification = object.classification;
75+
estimated_object.shape = object.shape;
76+
estimated_object.kinematics.pose_with_covariance.pose.position.x =
77+
x + vx * std::cos(yaw) * dt.seconds();
78+
estimated_object.kinematics.pose_with_covariance.pose.position.y =
79+
y + vx * std::sin(yaw) * dt.seconds();
80+
estimated_object.kinematics.pose_with_covariance.pose.position.z = z;
81+
const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds());
82+
estimated_object.kinematics.pose_with_covariance.pose.orientation =
83+
autoware::universe_utils::createQuaternionFromYaw(yaw_hat);
84+
output.objects.push_back(estimated_object);
85+
}
86+
return true;
87+
}
88+
} // namespace autoware::detection_by_tracker

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

+22-13
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 TIER IV, Inc.
1+
// Copyright 2024 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,22 +12,31 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "utils.hpp"
15+
#ifndef TRACKER__TRACKER_HANDLER_HPP_
16+
#define TRACKER__TRACKER_HANDLER_HPP_
1617

17-
#include <autoware_perception_msgs/msg/object_classification.hpp>
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include "autoware_perception_msgs/msg/tracked_objects.hpp"
21+
22+
#include <deque>
1823

1924
namespace autoware::detection_by_tracker
2025
{
21-
namespace utils
22-
{
23-
using Label = autoware_perception_msgs::msg::ObjectClassification;
2426

25-
bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const
27+
class TrackerHandler
2628
{
27-
return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) ||
28-
(label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) ||
29-
(label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) ||
30-
(label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN);
31-
}
32-
} // namespace utils
29+
private:
30+
std::deque<autoware_perception_msgs::msg::TrackedObjects> objects_buffer_;
31+
32+
public:
33+
TrackerHandler() = default;
34+
void onTrackedObjects(
35+
const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg);
36+
bool estimateTrackedObjects(
37+
const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output);
38+
};
39+
3340
} // namespace autoware::detection_by_tracker
41+
42+
#endif // TRACKER__TRACKER_HANDLER_HPP_

0 commit comments

Comments
 (0)