Skip to content

Commit 4b1f18e

Browse files
technolojintechnolojin
authored
fix(multi_object_tracker): huge tracking object bug fix for x2 (#1314)
* fix: catch-up changes Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: hug object bug fix, limit object size Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: node config fix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 2a1dad5 commit 4b1f18e

29 files changed

+3546
-2397
lines changed

‎perception/multi_object_tracker/CMakeLists.txt

+15-6
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ endif()
1212
### Find Eigen Dependencies
1313
find_package(eigen3_cmake_module REQUIRED)
1414
find_package(Eigen3 REQUIRED)
15+
find_package(glog REQUIRED)
1516

1617
include_directories(
1718
SYSTEM
@@ -21,6 +22,15 @@ include_directories(
2122
# Generate exe file
2223
set(MULTI_OBJECT_TRACKER_SRC
2324
src/multi_object_tracker_core.cpp
25+
src/debugger.cpp
26+
src/processor/processor.cpp
27+
src/data_association/data_association.cpp
28+
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
29+
src/tracker/motion_model/motion_model_base.cpp
30+
src/tracker/motion_model/bicycle_motion_model.cpp
31+
# cspell: ignore ctrv
32+
src/tracker/motion_model/ctrv_motion_model.cpp
33+
src/tracker/motion_model/cv_motion_model.cpp
2434
src/tracker/model/tracker_base.cpp
2535
src/tracker/model/big_vehicle_tracker.cpp
2636
src/tracker/model/normal_vehicle_tracker.cpp
@@ -30,21 +40,20 @@ set(MULTI_OBJECT_TRACKER_SRC
3040
src/tracker/model/pedestrian_and_bicycle_tracker.cpp
3141
src/tracker/model/unknown_tracker.cpp
3242
src/tracker/model/pass_through_tracker.cpp
33-
src/data_association/data_association.cpp
34-
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
3543
)
3644

37-
ament_auto_add_library(multi_object_tracker_node SHARED
45+
ament_auto_add_library(${PROJECT_NAME} SHARED
3846
${MULTI_OBJECT_TRACKER_SRC}
3947
)
4048

41-
target_link_libraries(multi_object_tracker_node
49+
target_link_libraries(${PROJECT_NAME}
4250
Eigen3::Eigen
51+
glog::glog
4352
)
4453

45-
rclcpp_components_register_node(multi_object_tracker_node
54+
rclcpp_components_register_node(${PROJECT_NAME}
4655
PLUGIN "MultiObjectTracker"
47-
EXECUTABLE multi_object_tracker
56+
EXECUTABLE ${PROJECT_NAME}_node
4857
)
4958

5059
ament_auto_package(INSTALL_TO_SHARE

‎perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2024 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+
//
16+
17+
#ifndef MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
18+
#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
19+
20+
#include <diagnostic_updater/diagnostic_updater.hpp>
21+
#include <diagnostic_updater/publisher.hpp>
22+
#include <rclcpp/rclcpp.hpp>
23+
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
24+
// #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
25+
26+
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
27+
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
28+
#include <geometry_msgs/msg/pose_stamped.hpp>
29+
30+
#include <memory>
31+
32+
/**
33+
* @brief Debugger class for multi object tracker
34+
* @details This class is used to publish debug information of multi object tracker
35+
*/
36+
class TrackerDebugger
37+
{
38+
public:
39+
explicit TrackerDebugger(rclcpp::Node & node);
40+
void publishTentativeObjects(
41+
const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
42+
void startMeasurementTime(
43+
const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp);
44+
void endMeasurementTime(const rclcpp::Time & now);
45+
void startPublishTime(const rclcpp::Time & now);
46+
void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
47+
48+
void setupDiagnostics();
49+
void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
50+
struct DEBUG_SETTINGS
51+
{
52+
bool publish_processing_time;
53+
bool publish_tentative_objects;
54+
double diagnostics_warn_delay;
55+
double diagnostics_error_delay;
56+
} debug_settings_;
57+
double pipeline_latency_ms_ = 0.0;
58+
diagnostic_updater::Updater diagnostic_updater_;
59+
bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
60+
61+
private:
62+
void loadParameters();
63+
bool is_initialized_ = false;
64+
rclcpp::Node & node_;
65+
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
66+
debug_tentative_objects_pub_;
67+
std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
68+
rclcpp::Time last_input_stamp_;
69+
rclcpp::Time stamp_process_start_;
70+
rclcpp::Time stamp_process_end_;
71+
rclcpp::Time stamp_publish_start_;
72+
rclcpp::Time stamp_publish_output_;
73+
};
74+
75+
#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_

‎perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

+20-59
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,11 @@
2020
#define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_
2121

2222
#include "multi_object_tracker/data_association/data_association.hpp"
23+
#include "multi_object_tracker/debugger.hpp"
24+
#include "multi_object_tracker/processor/processor.hpp"
2325
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
2426

2527
#include <rclcpp/rclcpp.hpp>
26-
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
27-
#include <tier4_autoware_utils/system/stop_watch.hpp>
2828

2929
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
3030
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -40,91 +40,52 @@
4040
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
4141
#endif
4242

43-
#include <diagnostic_updater/diagnostic_updater.hpp>
44-
#include <diagnostic_updater/publisher.hpp>
45-
4643
#include <tf2_ros/buffer.h>
4744
#include <tf2_ros/transform_listener.h>
4845

4946
#include <list>
5047
#include <map>
5148
#include <memory>
5249
#include <string>
50+
#include <unordered_map>
5351
#include <vector>
5452

55-
/**
56-
* @brief Debugger class for multi object tracker
57-
* @details This class is used to publish debug information of multi object tracker
58-
*/
59-
class TrackerDebugger
60-
{
61-
public:
62-
explicit TrackerDebugger(rclcpp::Node & node);
63-
void publishProcessingTime();
64-
void publishTentativeObjects(
65-
const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
66-
void startStopWatch();
67-
void startMeasurementTime(const rclcpp::Time & measurement_header_stamp);
68-
void setupDiagnostics();
69-
void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
70-
struct DEBUG_SETTINGS
71-
{
72-
bool publish_processing_time;
73-
bool publish_tentative_objects;
74-
double diagnostics_warn_delay;
75-
double diagnostics_error_delay;
76-
} debug_settings_;
77-
double elapsed_time_from_sensor_input_ = 0.0;
78-
diagnostic_updater::Updater diagnostic_updater_;
79-
80-
private:
81-
void loadParameters();
82-
rclcpp::Node & node_;
83-
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
84-
debug_tentative_objects_pub_;
85-
std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
86-
std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
87-
rclcpp::Time last_input_stamp_;
88-
};
89-
9053
class MultiObjectTracker : public rclcpp::Node
9154
{
9255
public:
9356
explicit MultiObjectTracker(const rclcpp::NodeOptions & node_options);
9457

9558
private:
59+
// ROS interface
9660
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
9761
tracked_objects_pub_;
9862
rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
9963
detected_object_sub_;
100-
rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer
101-
102-
// debugger class
103-
std::unique_ptr<TrackerDebugger> debugger_;
104-
10564
tf2_ros::Buffer tf_buffer_;
10665
tf2_ros::TransformListener tf_listener_;
10766

108-
std::map<std::uint8_t, std::string> tracker_map_;
67+
// debugger
68+
std::unique_ptr<TrackerDebugger> debugger_;
69+
// std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
10970

110-
void onMeasurement(
111-
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
112-
void onTimer();
71+
// publish timer
72+
rclcpp::TimerBase::SharedPtr publish_timer_;
73+
rclcpp::Time last_published_time_;
74+
double publisher_period_;
11375

76+
// internal states
11477
std::string world_frame_id_; // tracking frame
115-
std::list<std::shared_ptr<Tracker>> list_tracker_;
11678
std::unique_ptr<DataAssociation> data_association_;
79+
std::unique_ptr<TrackerProcessor> processor_;
11780

118-
void checkTrackerLifeCycle(
119-
std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
120-
const geometry_msgs::msg::Transform & self_transform);
121-
void sanitizeTracker(
122-
std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
123-
std::shared_ptr<Tracker> createNewTracker(
124-
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
125-
const geometry_msgs::msg::Transform & self_transform) const;
81+
// callback functions
82+
void onMeasurement(
83+
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
84+
void onTimer();
12685

127-
void publish(const rclcpp::Time & time);
86+
// publish processes
87+
void checkAndPublish(const rclcpp::Time & time);
88+
void publish(const rclcpp::Time & time) const;
12889
inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
12990
};
13091

‎perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// Copyright 2024 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+
//
16+
17+
#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_
18+
#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_
19+
20+
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
21+
22+
#include <rclcpp/rclcpp.hpp>
23+
24+
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
25+
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
26+
27+
#include <list>
28+
#include <map>
29+
#include <memory>
30+
#include <string>
31+
#include <unordered_map>
32+
#include <vector>
33+
34+
class TrackerProcessor
35+
{
36+
public:
37+
explicit TrackerProcessor(const std::map<std::uint8_t, std::string> & tracker_map);
38+
39+
const std::list<std::shared_ptr<Tracker>> & getListTracker() const { return list_tracker_; }
40+
// tracker processes
41+
void predict(const rclcpp::Time & time);
42+
void update(
43+
const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects,
44+
const geometry_msgs::msg::Transform & self_transform,
45+
const std::unordered_map<int, int> & direct_assignment);
46+
void spawn(
47+
const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
48+
const geometry_msgs::msg::Transform & self_transform,
49+
const std::unordered_map<int, int> & reverse_assignment);
50+
void prune(const rclcpp::Time & time);
51+
52+
// output
53+
bool isConfidentTracker(const std::shared_ptr<Tracker> & tracker) const;
54+
void getTrackedObjects(
55+
const rclcpp::Time & time,
56+
autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const;
57+
void getTentativeObjects(
58+
const rclcpp::Time & time,
59+
autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
60+
61+
private:
62+
std::map<std::uint8_t, std::string> tracker_map_;
63+
std::list<std::shared_ptr<Tracker>> list_tracker_;
64+
65+
// parameters
66+
float max_elapsed_time_; // [s]
67+
float min_iou_; // [ratio]
68+
float min_iou_for_unknown_object_; // [ratio]
69+
double distance_threshold_; // [m]
70+
int confident_count_threshold_; // [count]
71+
72+
void removeOldTracker(const rclcpp::Time & time);
73+
void removeOverlappedTracker(const rclcpp::Time & time);
74+
std::shared_ptr<Tracker> createNewTracker(
75+
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
76+
const geometry_msgs::msg::Transform & self_transform) const;
77+
};
78+
79+
#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_

0 commit comments

Comments
 (0)