forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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>
- Loading branch information
1 parent
2a1dad5
commit 4b1f18e
Showing
29 changed files
with
3,546 additions
and
2,397 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
75 changes: 75 additions & 0 deletions
75
perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,75 @@ | ||
| // Copyright 2024 Tier IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // | ||
| // | ||
|
|
||
| #ifndef MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ | ||
| #define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ | ||
|
|
||
| #include <diagnostic_updater/diagnostic_updater.hpp> | ||
| #include <diagnostic_updater/publisher.hpp> | ||
| #include <rclcpp/rclcpp.hpp> | ||
| #include <tier4_autoware_utils/ros/debug_publisher.hpp> | ||
| // #include <tier4_autoware_utils/ros/published_time_publisher.hpp> | ||
|
|
||
| #include <autoware_auto_perception_msgs/msg/detected_objects.hpp> | ||
| #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp> | ||
| #include <geometry_msgs/msg/pose_stamped.hpp> | ||
|
|
||
| #include <memory> | ||
|
|
||
| /** | ||
| * @brief Debugger class for multi object tracker | ||
| * @details This class is used to publish debug information of multi object tracker | ||
| */ | ||
| class TrackerDebugger | ||
| { | ||
| public: | ||
| explicit TrackerDebugger(rclcpp::Node & node); | ||
| void publishTentativeObjects( | ||
| const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; | ||
| void startMeasurementTime( | ||
| const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); | ||
| void endMeasurementTime(const rclcpp::Time & now); | ||
| void startPublishTime(const rclcpp::Time & now); | ||
| void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); | ||
|
|
||
| void setupDiagnostics(); | ||
| void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); | ||
| struct DEBUG_SETTINGS | ||
| { | ||
| bool publish_processing_time; | ||
| bool publish_tentative_objects; | ||
| double diagnostics_warn_delay; | ||
| double diagnostics_error_delay; | ||
| } debug_settings_; | ||
| double pipeline_latency_ms_ = 0.0; | ||
| diagnostic_updater::Updater diagnostic_updater_; | ||
| bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } | ||
|
|
||
| private: | ||
| void loadParameters(); | ||
| bool is_initialized_ = false; | ||
| rclcpp::Node & node_; | ||
| rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr | ||
| debug_tentative_objects_pub_; | ||
| std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_; | ||
| rclcpp::Time last_input_stamp_; | ||
| rclcpp::Time stamp_process_start_; | ||
| rclcpp::Time stamp_process_end_; | ||
| rclcpp::Time stamp_publish_start_; | ||
| rclcpp::Time stamp_publish_output_; | ||
| }; | ||
|
|
||
| #endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
79 changes: 79 additions & 0 deletions
79
perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,79 @@ | ||
| // Copyright 2024 Tier IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // | ||
| // | ||
|
|
||
| #ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ | ||
| #define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ | ||
|
|
||
| #include "multi_object_tracker/tracker/model/tracker_base.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <autoware_auto_perception_msgs/msg/detected_objects.hpp> | ||
| #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp> | ||
|
|
||
| #include <list> | ||
| #include <map> | ||
| #include <memory> | ||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <vector> | ||
|
|
||
| class TrackerProcessor | ||
| { | ||
| public: | ||
| explicit TrackerProcessor(const std::map<std::uint8_t, std::string> & tracker_map); | ||
|
|
||
| const std::list<std::shared_ptr<Tracker>> & getListTracker() const { return list_tracker_; } | ||
| // tracker processes | ||
| void predict(const rclcpp::Time & time); | ||
| void update( | ||
| const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects, | ||
| const geometry_msgs::msg::Transform & self_transform, | ||
| const std::unordered_map<int, int> & direct_assignment); | ||
| void spawn( | ||
| const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, | ||
| const geometry_msgs::msg::Transform & self_transform, | ||
| const std::unordered_map<int, int> & reverse_assignment); | ||
| void prune(const rclcpp::Time & time); | ||
|
|
||
| // output | ||
| bool isConfidentTracker(const std::shared_ptr<Tracker> & tracker) const; | ||
| void getTrackedObjects( | ||
| const rclcpp::Time & time, | ||
| autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const; | ||
| void getTentativeObjects( | ||
| const rclcpp::Time & time, | ||
| autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; | ||
|
|
||
| private: | ||
| std::map<std::uint8_t, std::string> tracker_map_; | ||
| std::list<std::shared_ptr<Tracker>> list_tracker_; | ||
|
|
||
| // parameters | ||
| float max_elapsed_time_; // [s] | ||
| float min_iou_; // [ratio] | ||
| float min_iou_for_unknown_object_; // [ratio] | ||
| double distance_threshold_; // [m] | ||
| int confident_count_threshold_; // [count] | ||
|
|
||
| void removeOldTracker(const rclcpp::Time & time); | ||
| void removeOverlappedTracker(const rclcpp::Time & time); | ||
| std::shared_ptr<Tracker> createNewTracker( | ||
| const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, | ||
| const geometry_msgs::msg::Transform & self_transform) const; | ||
| }; | ||
|
|
||
| #endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ |
Oops, something went wrong.