From 26e31612b71d861a71da676282add3aac0876a3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 10:00:16 +0900 Subject: [PATCH 01/23] feat: Add dynamic_object.hpp to object_model directory Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 70 +++++++++++++++++++ .../src/multi_object_tracker_node.hpp | 1 + 2 files changed, 71 insertions(+) create mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp new file mode 100644 index 0000000000000..d640bc2afeaf8 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -0,0 +1,70 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace types +{ +struct ObjectKinematics +{ + uint8_t UNAVAILABLE = 0; + uint8_t SIGN_UNKNOWN = 1; + uint8_t AVAILABLE = 2; + + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + bool has_position_covariance = false; + uint8_t orientation_availability; + bool has_twist = false; + bool has_twist_covariance = false; +}; + +struct ObjectShape +{ + uint8_t BOUNDING_BOX = 0; + uint8_t CYLINDER = 1; + uint8_t POLYGON = 2; + + uint8_t type; + geometry_msgs::msg::Polygon footprint; + geometry_msgs::msg::Vector3 dimensions; +}; + +struct DynamicObject +{ + float_t existence_probability; + std::vector classification; + ObjectKinematics kinematics; + ObjectShape shape; +}; + +} // namespace types +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 79a8c1b98dcca..ca36703cf3dbd 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER_NODE_HPP_ #include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "debugger/debugger.hpp" #include "processor/input_manager.hpp" From c0541549d528e5fdc25ce318af8240de4605042f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 10:37:50 +0900 Subject: [PATCH 02/23] chore: Update autoware_perception_msgs include statements in association.hpp and dynamic_object.hpp Signed-off-by: Taekjin LEE --- .../association/association.hpp | 2 +- .../object_model/dynamic_object.hpp | 85 ++++++++++++++++--- 2 files changed, 76 insertions(+), 11 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 2c12341d0aa67..81811efcb483f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -27,7 +27,7 @@ #include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index d640bc2afeaf8..056f4158e1594 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -19,11 +19,17 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ +#include +#include +#include #include +#include +#include #include #include #include #include +#include #include @@ -31,27 +37,31 @@ namespace autoware::multi_object_tracker { namespace types { +enum OrientationAvailability : uint8_t { + UNAVAILABLE = 0, + SIGN_UNKNOWN = 1, + AVAILABLE = 2, +}; + +enum ShapeType : uint8_t { + BOUNDING_BOX = 0, + CYLINDER = 1, + POLYGON = 2, +}; + struct ObjectKinematics { - uint8_t UNAVAILABLE = 0; - uint8_t SIGN_UNKNOWN = 1; - uint8_t AVAILABLE = 2; - geometry_msgs::msg::PoseWithCovariance pose_with_covariance; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; bool has_position_covariance = false; - uint8_t orientation_availability; + OrientationAvailability orientation_availability; bool has_twist = false; bool has_twist_covariance = false; }; struct ObjectShape { - uint8_t BOUNDING_BOX = 0; - uint8_t CYLINDER = 1; - uint8_t POLYGON = 2; - - uint8_t type; + ShapeType type; geometry_msgs::msg::Polygon footprint; geometry_msgs::msg::Vector3 dimensions; }; @@ -64,6 +74,61 @@ struct DynamicObject ObjectShape shape; }; +struct DynamicObjects +{ + std_msgs::msg::Header header; + std::vector objects; +}; + +DynamicObject getDynamicObject(autoware_perception_msgs::msg::DetectedObject object) +{ + DynamicObject dynamic_object; + dynamic_object.existence_probability = object.existence_probability; + dynamic_object.classification = object.classification; + + dynamic_object.kinematics.pose_with_covariance = object.kinematics.pose_with_covariance; + dynamic_object.kinematics.twist_with_covariance = object.kinematics.twist_with_covariance; + dynamic_object.kinematics.has_position_covariance = object.kinematics.has_position_covariance; + if ( + object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; + } else if ( + object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; + } else if ( + object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; + } + dynamic_object.kinematics.has_twist = object.kinematics.has_twist; + dynamic_object.kinematics.has_twist_covariance = object.kinematics.has_twist_covariance; + + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + dynamic_object.shape.type = ShapeType::BOUNDING_BOX; + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + dynamic_object.shape.type = ShapeType::CYLINDER; + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + dynamic_object.shape.type = ShapeType::POLYGON; + } + + dynamic_object.shape.footprint = object.shape.footprint; + dynamic_object.shape.dimensions = object.shape.dimensions; + return dynamic_object; +} + +DynamicObjects getDynamicObject(autoware_perception_msgs::msg::DetectedObjects objects) +{ + DynamicObjects dynamic_objects; + dynamic_objects.header = objects.header; + dynamic_objects.objects.reserve(objects.objects.size()); + for (const auto & object : objects.objects) { + dynamic_objects.objects.emplace_back(getDynamicObject(object)); + } + return dynamic_objects; +} + } // namespace types } // namespace autoware::multi_object_tracker From d1a9698ee0bb4031466fd22b8315203514d8f40b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 13:00:11 +0900 Subject: [PATCH 03/23] fix: replace object message type to the DynamicObject type Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 1 + .../association/association.hpp | 2 +- .../object_model/dynamic_object.hpp | 69 ++----- .../tracker/model/bicycle_tracker.hpp | 18 +- .../model/multiple_vehicle_tracker.hpp | 7 +- .../tracker/model/pass_through_tracker.hpp | 12 +- .../model/pedestrian_and_bicycle_tracker.hpp | 8 +- .../tracker/model/pedestrian_tracker.hpp | 18 +- .../tracker/model/tracker_base.hpp | 14 +- .../tracker/model/unknown_tracker.hpp | 19 +- .../tracker/model/vehicle_tracker.hpp | 22 +-- .../uncertainty/uncertainty_processor.hpp | 11 +- .../multi_object_tracker/utils/utils.hpp | 31 ++- .../lib/association/association.cpp | 12 +- .../lib/object_model/dynamic_object.cpp | 181 ++++++++++++++++++ .../lib/tracker/model/bicycle_tracker.cpp | 25 ++- .../model/multiple_vehicle_tracker.cpp | 4 +- .../tracker/model/pass_through_tracker.cpp | 6 +- .../model/pedestrian_and_bicycle_tracker.cpp | 4 +- .../lib/tracker/model/pedestrian_tracker.cpp | 37 ++-- .../lib/tracker/model/tracker_base.cpp | 5 +- .../lib/tracker/model/unknown_tracker.cpp | 18 +- .../lib/tracker/model/vehicle_tracker.cpp | 33 ++-- .../lib/uncertainty/uncertainty_processor.cpp | 17 +- .../src/debugger/debug_object.cpp | 3 +- .../src/debugger/debug_object.hpp | 3 +- .../src/debugger/debugger.cpp | 3 +- .../src/debugger/debugger.hpp | 3 +- .../src/multi_object_tracker_node.cpp | 9 +- .../src/multi_object_tracker_node.hpp | 2 +- .../src/processor/input_manager.cpp | 18 +- .../src/processor/input_manager.hpp | 12 +- .../src/processor/processor.cpp | 6 +- .../src/processor/processor.hpp | 7 +- 34 files changed, 397 insertions(+), 243 deletions(-) create mode 100644 perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 298b6605192a5..a5526b3326a1b 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -30,6 +30,7 @@ set(${PROJECT_NAME}_src set(${PROJECT_NAME}_lib lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/object_model/dynamic_object.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 81811efcb483f..13eba0c4766d0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -58,7 +58,7 @@ class DataAssociation const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjects & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 056f4158e1594..fc5cf8c595036 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -25,12 +25,20 @@ #include #include #include +#include #include #include #include #include #include +#include +#include + +#include +#include + +#include #include namespace autoware::multi_object_tracker @@ -80,56 +88,21 @@ struct DynamicObjects std::vector objects; }; -DynamicObject getDynamicObject(autoware_perception_msgs::msg::DetectedObject object) -{ - DynamicObject dynamic_object; - dynamic_object.existence_probability = object.existence_probability; - dynamic_object.classification = object.classification; - - dynamic_object.kinematics.pose_with_covariance = object.kinematics.pose_with_covariance; - dynamic_object.kinematics.twist_with_covariance = object.kinematics.twist_with_covariance; - dynamic_object.kinematics.has_position_covariance = object.kinematics.has_position_covariance; - if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { - dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; - } else if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { - dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; - } else if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { - dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; - } - dynamic_object.kinematics.has_twist = object.kinematics.has_twist; - dynamic_object.kinematics.has_twist_covariance = object.kinematics.has_twist_covariance; - - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - dynamic_object.shape.type = ShapeType::BOUNDING_BOX; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - dynamic_object.shape.type = ShapeType::CYLINDER; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - dynamic_object.shape.type = ShapeType::POLYGON; - } - - dynamic_object.shape.footprint = object.shape.footprint; - dynamic_object.shape.dimensions = object.shape.dimensions; - return dynamic_object; -} - -DynamicObjects getDynamicObject(autoware_perception_msgs::msg::DetectedObjects objects) -{ - DynamicObjects dynamic_objects; - dynamic_objects.header = objects.header; - dynamic_objects.objects.reserve(objects.objects.size()); - for (const auto & object : objects.objects) { - dynamic_objects.objects.emplace_back(getDynamicObject(object)); - } - return dynamic_objects; -} +DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); + + +DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); + +autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object); } // namespace types + + +bool transformObjects( + const types::DynamicObjects & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); + + double getArea(const types::ObjectShape & shape); } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8501c68b0cf23..847520cb05cc9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class BicycleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::bicycle; @@ -50,23 +52,23 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 8b4fa1babf652..0cf38a3f38529 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,10 +19,11 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" +#include #include namespace autoware::multi_object_tracker @@ -36,13 +37,13 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 45cd0f31a4e85..ae34984e65b56 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,28 +19,30 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { class PassThroughTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; - autoware_perception_msgs::msg::DetectedObject prev_observed_object_; + types::DynamicObject object_; + types::DynamicObject prev_observed_object_; rclcpp::Logger logger_; rclcpp::Time last_update_time_; public: PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 4287e0f99d5ee..e0063f66dd2d9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,13 +37,13 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index f20b38f73e95f..664e3125d5f3d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class PedestrianTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::pedestrian; @@ -56,23 +58,23 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 0b9ea9c44a6cf..e1d5698d9cf68 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,14 +20,15 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include +#include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include +#include #include #include @@ -67,9 +68,8 @@ class Tracker return existence_vector.size() > 0; } bool updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index); + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification @@ -108,7 +108,7 @@ class Tracker protected: virtual bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; public: diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 9f128c864ad6c..b4ab15673a057 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,17 +19,19 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class UnknownTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; struct EkfParams @@ -47,19 +49,18 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index f3708fd1ff905..09d775eee48cc 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,7 +37,7 @@ class VehicleTracker : public Tracker double velocity_deviation_threshold_; - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; double z_; struct BoundingBox @@ -53,24 +55,22 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, + const size_t channel_size, const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 12bd865795b85..efcd7a1901ba6 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -18,6 +18,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include @@ -32,21 +33,19 @@ namespace uncertainty { using autoware::multi_object_tracker::object_model::ObjectModel; -using autoware_perception_msgs::msg::DetectedObject; -using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; ObjectModel decodeObjectModel(const ObjectClassification & object_class); -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); +types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_objects); object_model::StateCovariance covarianceFromObjectClass( - const DetectedObject & detected_object, const ObjectClassification & object_class); + const types::DynamicObject & detected_object, const ObjectClassification & object_class); -void normalizeUncertainty(DetectedObjects & detected_objects); +void normalizeUncertainty(types::DynamicObjects & detected_objects); -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects); +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjects & detected_objects); } // namespace uncertainty } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 833f768e171f4..becc6e379ea5e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -19,12 +19,14 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" + #include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/shape.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include +#include +#include #include #include #include @@ -36,6 +38,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace utils { enum BBOX_IDX { @@ -172,9 +176,8 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( * @return nearest corner index(int) */ inline void calcAnchorPointOffset( - const double w, const double l, const int indx, - const autoware_perception_msgs::msg::DetectedObject & input_object, const double & yaw, - autoware_perception_msgs::msg::DetectedObject & offset_object, Eigen::Vector2d & tracking_offset) + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) { // copy value offset_object = input_object; @@ -204,8 +207,7 @@ inline void calcAnchorPointOffset( * @param output_object: output bounding box objects */ inline bool convertConvexHullToBoundingBox( - const autoware_perception_msgs::msg::DetectedObject & input_object, - autoware_perception_msgs::msg::DetectedObject & output_object) + const types::DynamicObject & input_object, types::DynamicObject & output_object) { // check footprint size if (input_object.shape.footprint.points.size() < 3) { @@ -240,7 +242,7 @@ inline bool convertConvexHullToBoundingBox( output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.type = types::ShapeType::BOUNDING_BOX; output_object.shape.dimensions.x = max_x - min_x; output_object.shape.dimensions.y = max_y - min_y; output_object.shape.dimensions.z = max_z; @@ -249,16 +251,13 @@ inline bool convertConvexHullToBoundingBox( } inline bool getMeasurementYaw( - const autoware_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, - double & measurement_yaw) + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) { measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // check orientation sign is known or not, and fix the limiting delta yaw double limiting_delta_yaw = M_PI_2; - if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { limiting_delta_yaw = M_PI; } // limiting delta yaw, even the availability is unknown @@ -270,10 +269,10 @@ inline bool getMeasurementYaw( } } // return false if the orientation is unknown - return object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; } } // namespace utils +} // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 0c809ee5f086d..08bb24abddc56 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -15,6 +15,7 @@ #include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" @@ -150,8 +151,7 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, - const std::list> & trackers) + const types::DynamicObjects & measurements, const std::list> & trackers) { Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), measurements.objects.size()); @@ -162,8 +162,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); ++measurement_idx) { - const autoware_perception_msgs::msg::DetectedObject & measurement_object = - measurements.objects.at(measurement_idx); + const types::DynamicObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); @@ -186,7 +185,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware::universe_utils::getArea(measurement_object.shape); + const double area = getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate @@ -210,8 +209,9 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; + const auto meas_obj = types::getTrackedObject(measurement_object); const double iou = autoware::object_recognition_utils::get2dIoU( - measurement_object, tracked_object, min_union_iou_area); + meas_obj, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp new file mode 100644 index 0000000000000..3d3bb2808715f --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -0,0 +1,181 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" + +namespace autoware::multi_object_tracker +{ + +namespace types +{ + +DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object) +{ + DynamicObject dynamic_object; + dynamic_object.existence_probability = det_object.existence_probability; + dynamic_object.classification = det_object.classification; + + dynamic_object.kinematics.pose_with_covariance = det_object.kinematics.pose_with_covariance; + dynamic_object.kinematics.twist_with_covariance = det_object.kinematics.twist_with_covariance; + dynamic_object.kinematics.has_position_covariance = det_object.kinematics.has_position_covariance; + if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; + } + dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; + dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; + + if (det_object.shape.type == types::ShapeType::BOUNDING_BOX) { + dynamic_object.shape.type = ShapeType::BOUNDING_BOX; + } else if (det_object.shape.type == types::ShapeType::CYLINDER) { + dynamic_object.shape.type = ShapeType::CYLINDER; + } else if (det_object.shape.type == types::ShapeType::POLYGON) { + dynamic_object.shape.type = ShapeType::POLYGON; + } + + dynamic_object.shape.footprint = det_object.shape.footprint; + dynamic_object.shape.dimensions = det_object.shape.dimensions; + return dynamic_object; +}; + +DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects) +{ + DynamicObjects dynamic_objects; + dynamic_objects.header = det_objects.header; + dynamic_objects.objects.reserve(det_objects.objects.size()); + for (const auto & det_object : det_objects.objects) { + dynamic_objects.objects.emplace_back(getDynamicObject(det_object)); + } + return dynamic_objects; +}; + +autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object) +{ + autoware_perception_msgs::msg::TrackedObject tracked_object; + tracked_object.existence_probability = dyn_object.existence_probability; + tracked_object.classification = dyn_object.classification; + + tracked_object.kinematics.pose_with_covariance = dyn_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = dyn_object.kinematics.twist_with_covariance; + if (dyn_object.kinematics.orientation_availability == OrientationAvailability::UNAVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } else if ( + dyn_object.kinematics.orientation_availability == OrientationAvailability::SIGN_UNKNOWN) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else if (dyn_object.kinematics.orientation_availability == OrientationAvailability::AVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::AVAILABLE; + } + tracked_object.kinematics.is_stationary = false; + + if (dyn_object.shape.type == ShapeType::BOUNDING_BOX) { + tracked_object.shape.type = types::ShapeType::BOUNDING_BOX; + } else if (dyn_object.shape.type == ShapeType::CYLINDER) { + tracked_object.shape.type = types::ShapeType::CYLINDER; + } else if (dyn_object.shape.type == ShapeType::POLYGON) { + tracked_object.shape.type = types::ShapeType::POLYGON; + } + + tracked_object.shape.footprint = dyn_object.shape.footprint; + tracked_object.shape.dimensions = dyn_object.shape.dimensions; + return tracked_object; +}; + +} // namespace types + + + + + +inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return boost::none; + } +}; + +bool transformObjects( + const types::DynamicObjects & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + return true; +}; + +double getArea(const types::ObjectShape & shape) +{ + if (shape.type == types::ShapeType::BOUNDING_BOX) { + return shape.dimensions.x * shape.dimensions.y; + } else if (shape.type == types::ShapeType::CYLINDER) { + return M_PI * shape.dimensions.x * shape.dimensions.x * 0.25; + } else if (shape.type == types::ShapeType::POLYGON) { + double area = 0.0; + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + const auto & p1 = shape.footprint.points.at(i); + const auto & p2 = shape.footprint.points.at((i + 1) % shape.footprint.points.size()); + area += (p1.x * p2.y - p2.x * p1.y) / 2.0; + } + return std::abs(area); + } + return 0.0; +}; +} // namespace autoware::multi_object_tracker \ No newline at end of file diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 69a9edd9c9d9e..3069c26f4e757 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -46,7 +46,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -59,7 +59,7 @@ BicycleTracker::BicycleTracker( initializeExistenceProbabilities(channel_index, object.existence_probability); // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == types::ShapeType::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { @@ -148,15 +148,15 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject BicycleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != types::ShapeType::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } @@ -165,7 +165,7 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( return updating_object; } -bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); @@ -196,9 +196,9 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithShape(const types::DynamicObject & object) { - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != types::ShapeType::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } @@ -235,7 +235,7 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool BicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -260,8 +260,7 @@ bool BicycleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -271,7 +270,7 @@ bool BicycleTracker::measure( bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = types::getTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index ff06544bd509c..f4de5e13d6e23 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -26,7 +26,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -47,7 +47,7 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) } bool MultipleVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { big_vehicle_tracker_.measure(object, time, self_transform); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index f66e616241ae0..17d5c0c9e26ca 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -40,7 +40,7 @@ namespace autoware::multi_object_tracker { PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -66,7 +66,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) } bool PassThroughTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { prev_observed_object_ = object_; @@ -91,7 +91,7 @@ bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = types::getTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 1b8018351f5a5..343d4631ee3f4 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -24,7 +24,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -43,7 +43,7 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { pedestrian_tracker_.measure(object, time, self_transform); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 2135514df8485..fc85b21493274 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -46,7 +46,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -63,12 +63,12 @@ PedestrianTracker::PedestrianTracker( object_model_.init_size.length, object_model_.init_size.width, object_model_.init_size.height}; // default value cylinder_ = {object_model_.init_size.length, object_model_.init_size.height}; // default value - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == types::ShapeType::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == types::ShapeType::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + } else if (object.shape.type == types::ShapeType::POLYGON) { // do not update polygon shape } // set maximum and minimum size @@ -148,17 +148,16 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject PedestrianTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; return updating_object; } -bool PedestrianTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -178,13 +177,12 @@ bool PedestrianTracker::measureWithPose( return is_updated; } -bool PedestrianTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) { constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == types::ShapeType::BOUNDING_BOX) { // check bound box size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] @@ -200,7 +198,7 @@ bool PedestrianTracker::measureWithShape( bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == types::ShapeType::CYLINDER) { // check cylinder size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] @@ -235,7 +233,7 @@ bool PedestrianTracker::measureWithShape( } bool PedestrianTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -259,8 +257,7 @@ bool PedestrianTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -271,7 +268,7 @@ bool PedestrianTracker::measure( bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = types::getTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); @@ -290,15 +287,15 @@ bool PedestrianTracker::getTrackedObject( pose_with_cov.pose.position.z = z_; // set shape - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == types::ShapeType::BOUNDING_BOX) { object.shape.dimensions.x = bounding_box_.length; object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == types::ShapeType::CYLINDER) { object.shape.dimensions.x = cylinder_.width; object.shape.dimensions.y = cylinder_.width; object.shape.dimensions.z = cylinder_.height; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + } else if (object.shape.type == types::ShapeType::POLYGON) { const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 24e2b0c9f5acf..2ec995c0e9620 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -81,9 +81,8 @@ void Tracker::initializeExistenceProbabilities( } bool Tracker::updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index) + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) { // Update existence probability { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 2805e43b88323..e18d300c83331 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -40,7 +40,7 @@ namespace autoware::multi_object_tracker { UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -142,11 +142,10 @@ bool UnknownTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +types::DynamicObject UnknownTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -169,7 +168,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( return updating_object; } -bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool UnknownTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -190,7 +189,7 @@ bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::Detect } bool UnknownTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -207,8 +206,7 @@ bool UnknownTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); return true; @@ -217,7 +215,7 @@ bool UnknownTracker::measure( bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = types::getTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 2d1f48a3ad5ee..716452e4fdb1d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -47,9 +47,8 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, + const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), @@ -67,11 +66,11 @@ VehicleTracker::VehicleTracker( velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == types::ShapeType::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; + types::DynamicObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -167,16 +166,15 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) +types::DynamicObject VehicleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object = object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + types::DynamicObject bbox_object = object; + if (object.shape.type != types::ShapeType::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -200,7 +198,7 @@ autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( return updating_object; } -bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -242,9 +240,9 @@ bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithShape(const types::DynamicObject & object) { - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != types::ShapeType::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } @@ -295,7 +293,7 @@ bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool VehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -320,8 +318,7 @@ bool VehicleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -331,7 +328,7 @@ bool VehicleTracker::measure( bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = types::getTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index ddfdc7ef7cb10..ab1849c00930b 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -55,10 +55,10 @@ object_model::StateCovariance covarianceFromObjectClass(const ObjectClassificati return obj_class_model.measurement_covariance; } -DetectedObject modelUncertaintyByClass( - const DetectedObject & object, const ObjectClassification & object_class) +types::DynamicObject modelUncertaintyByClass( + const types::DynamicObject & object, const ObjectClassification & object_class) { - DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // measurement noise covariance const object_model::StateCovariance measurement_covariance = @@ -87,8 +87,7 @@ DetectedObject modelUncertaintyByClass( pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; if (!is_yaw_available) { pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } @@ -103,9 +102,9 @@ DetectedObject modelUncertaintyByClass( return updating_object; } -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_objects) { - DetectedObjects updating_objects; + types::DynamicObjects updating_objects; updating_objects.header = detected_objects.header; for (const auto & object : detected_objects.objects) { if (object.kinematics.has_position_covariance) { @@ -119,7 +118,7 @@ DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) return updating_objects; } -void normalizeUncertainty(DetectedObjects & detected_objects) +void normalizeUncertainty(types::DynamicObjects & detected_objects) { constexpr double min_cov_dist = 1e-4; constexpr double min_cov_rad = 1e-6; @@ -140,7 +139,7 @@ void normalizeUncertainty(DetectedObjects & detected_objects) } } -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects) +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjects & detected_objects) { const auto & odom_pose = odometry.pose.pose; const auto & odom_pose_cov = odometry.pose.covariance; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index bc27525d85f55..35734b40ae2e9 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -80,8 +80,7 @@ void TrackerObjectDebugger::reset() void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const uint & channel_index, const types::DynamicObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index e564afc9fd9d0..37e1f56281c3e 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,8 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const uint & channel_index, const types::DynamicObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 9d830bb9e5b81..da956135c32b3 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -185,8 +185,7 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const uint & channel_index, const types::DynamicObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 2c05da0c999e2..9577eeee1b719 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,8 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const uint & channel_index, const types::DynamicObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index d2d2d3e087c4e..860119da0df6b 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -241,7 +241,7 @@ void MultiObjectTracker::onTrigger() last_updated_time_ = current_time; const rclcpp::Time latest_time(objects_list.back().second.header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); - // run process for each DetectedObjects + // run process for each DynamicObjects for (const auto & objects_data : objects_list) { runProcess(objects_data.second, objects_data.first); } @@ -279,7 +279,7 @@ void MultiObjectTracker::onTimer() } void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects, const uint & channel_index) + const types::DynamicObjects & input_objects, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = @@ -293,9 +293,8 @@ void MultiObjectTracker::runProcess( } // Transform the objects to the world frame - DetectedObjects transformed_objects; - if (!autoware::object_recognition_utils::transformObjects( - input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { + types::DynamicObjects transformed_objects; + if (!transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index ca36703cf3dbd..b961755c53fd0 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -101,7 +101,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const DetectedObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjects & input_objects, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index d5b12ed7b2b82..0f284d97b5c99 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -14,6 +14,8 @@ #include "input_manager.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" + #include #include @@ -53,10 +55,12 @@ void InputStream::init(const InputChannel & input_channel) void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - const DetectedObjects & objects = *msg; + const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; + + types::DynamicObjects dynamic_objects = types::getDynamicObjects(objects); // Model the object uncertainty only if it is not available - DetectedObjects objects_with_uncertainty = uncertainty::modelUncertainty(objects); + types::DynamicObjects objects_with_uncertainty = uncertainty::modelUncertainty(dynamic_objects); // Move the objects_with_uncertainty to the objects queue objects_que_.push_back(std::move(objects_with_uncertainty)); @@ -167,7 +171,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - std::pair objects_pair(index_, objects); + std::pair objects_pair(index_, objects); objects_list.push_back(objects_pair); } } @@ -216,9 +220,9 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function func = + std::function func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = node_.create_subscription( + sub_objects_array_.at(i) = node_.create_subscription( input_channels[i].input_topic, rclcpp::QoS{1}, func); } @@ -339,7 +343,9 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const std::pair & a, const std::pair & b) { + []( + const std::pair & a, + const std::pair & b) { return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < 0; }); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 01b3148251743..64a8c29eecde0 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -15,9 +15,10 @@ #ifndef PROCESSOR__INPUT_MANAGER_HPP_ #define PROCESSOR__INPUT_MANAGER_HPP_ +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -28,8 +29,7 @@ namespace autoware::multi_object_tracker { -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using ObjectsList = std::vector>; +using ObjectsList = std::vector>; struct InputChannel { @@ -82,11 +82,10 @@ class InputStream bool is_spawn_enabled_{}; size_t que_size_{30}; - std::deque objects_que_; + std::deque objects_que_; std::function func_trigger_; - // bool is_time_initialized_{false}; int initial_count_{0}; double latency_mean_{}; double latency_var_{}; @@ -115,7 +114,8 @@ class InputManager private: rclcpp::Node & node_; - std::vector::SharedPtr> sub_objects_array_{}; + std::vector::SharedPtr> + sub_objects_array_{}; bool is_initialized_{false}; rclcpp::Time latest_exported_object_time_; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index b3bcd018faf9d..feec11bfa157a 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -44,7 +44,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } void TrackerProcessor::update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment, const uint & channel_index) { @@ -64,7 +64,7 @@ void TrackerProcessor::update( } void TrackerProcessor::spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment, const uint & channel_index) { @@ -81,7 +81,7 @@ void TrackerProcessor::spawn( } std::shared_ptr TrackerProcessor::createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { const LabelType label = diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 80ca92bc43671..fe0663a8687c9 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -15,6 +15,7 @@ #ifndef PROCESSOR__PROCESSOR_HPP_ #define PROCESSOR__PROCESSOR_HPP_ +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -53,11 +54,11 @@ class TrackerProcessor // tracker processes void predict(const rclcpp::Time & time); void update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment, const uint & channel_index); void prune(const rclcpp::Time & time); @@ -79,7 +80,7 @@ class TrackerProcessor void removeOldTracker(const rclcpp::Time & time); void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; From 013265f53101d24ffc07c2f2874fe5ca78af40e7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 13:55:26 +0900 Subject: [PATCH 04/23] chore: Update autoware_perception_msgs include statements in association.hpp and dynamic_object.hpp Signed-off-by: Taekjin LEE --- .../multi_object_tracker/object_model/dynamic_object.hpp | 1 - .../lib/object_model/dynamic_object.cpp | 4 ---- .../src/multi_object_tracker_node.cpp | 6 +++--- .../src/multi_object_tracker_node.hpp | 8 ++------ 4 files changed, 5 insertions(+), 14 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index fc5cf8c595036..eaac604d41d77 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -90,7 +90,6 @@ struct DynamicObjects DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); - DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object); diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 3d3bb2808715f..9838fa640666f 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -105,13 +105,9 @@ autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObjec tracked_object.shape.dimensions = dyn_object.shape.dimensions; return tracked_object; }; - } // namespace types - - - inline boost::optional getTransform( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 860119da0df6b..efd9c0952a2d7 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -95,7 +95,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -386,7 +386,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - TrackedObjects output_msg, tentative_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -398,7 +398,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_output_msg; + autoware_perception_msgs::msg::TrackedObjects tentative_output_msg; tentative_output_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_output_msg); debugger_->publishTentativeObjects(tentative_output_msg); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index b961755c53fd0..5a827889978f8 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -56,10 +56,6 @@ namespace autoware::multi_object_tracker { -using DetectedObject = autoware_perception_msgs::msg::DetectedObject; -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; - class MultiObjectTracker : public rclcpp::Node { public: @@ -67,8 +63,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; From 8cf9b83f23f8bc5fc87240ccaa28dfa93b081c42 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 15:01:14 +0900 Subject: [PATCH 05/23] chore: add channel index to the DynamicObjects Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 10 ++++----- .../lib/object_model/dynamic_object.cpp | 9 ++++++-- .../src/multi_object_tracker_node.cpp | 17 +++++++------- .../src/multi_object_tracker_node.hpp | 5 +++-- .../src/processor/input_manager.cpp | 22 +++++++++---------- .../src/processor/input_manager.hpp | 2 +- .../src/processor/processor.cpp | 9 ++++---- .../src/processor/processor.hpp | 4 ++-- 8 files changed, 41 insertions(+), 37 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index eaac604d41d77..9b4887f25cf13 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -31,12 +31,10 @@ #include #include #include -#include #include #include -#include #include #include @@ -85,23 +83,25 @@ struct DynamicObject struct DynamicObjects { std_msgs::msg::Header header; + uint8_t channel_index; std::vector objects; }; DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); -DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); +DynamicObjects getDynamicObjects( + const autoware_perception_msgs::msg::DetectedObjects & det_objects); autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object); } // namespace types - bool transformObjects( const types::DynamicObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); - double getArea(const types::ObjectShape & shape); +double getArea(const types::ObjectShape & shape); + } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 9838fa640666f..48058b8508f67 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -17,6 +17,12 @@ #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include + +#include + +#include + namespace autoware::multi_object_tracker { @@ -107,7 +113,6 @@ autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObjec }; } // namespace types - inline boost::optional getTransform( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) @@ -174,4 +179,4 @@ double getArea(const types::ObjectShape & shape) } return 0.0; }; -} // namespace autoware::multi_object_tracker \ No newline at end of file +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index efd9c0952a2d7..2ddae0548cd6a 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -95,7 +95,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -239,18 +240,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); // run process for each DynamicObjects for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); + runProcess(objects_data); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); checkAndPublish(latest_object_time); } } @@ -278,9 +279,9 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess( - const types::DynamicObjects & input_objects, const uint & channel_index) +void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) { + const uint & channel_index = input_objects.channel_index; // Get the time of the measurement const rclcpp::Time measurement_time = rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); @@ -354,14 +355,14 @@ void MultiObjectTracker::runProcess( } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + processor_->update(transformed_objects, *self_transform, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + processor_->spawn(transformed_objects, *self_transform, reverse_assignment); } } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 5a827889978f8..903779905f2c5 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -64,7 +64,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -97,7 +98,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const types::DynamicObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjects & input_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 0f284d97b5c99..34edd6e25d677 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -58,6 +58,7 @@ void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; types::DynamicObjects dynamic_objects = types::getDynamicObjects(objects); + dynamic_objects.channel_index = index_; // Model the object uncertainty only if it is not available types::DynamicObjects objects_with_uncertainty = uncertainty::modelUncertainty(dynamic_objects); @@ -171,8 +172,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - std::pair objects_pair(index_, objects); - objects_list.push_back(objects_pair); + objects_list.push_back(objects); } } @@ -220,10 +220,11 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function func = - std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = node_.create_subscription( - input_channels[i].input_topic, rclcpp::QoS{1}, func); + std::function + func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = + node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } // Check if any spawn enabled input streams @@ -343,17 +344,14 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - []( - const std::pair & a, - const std::pair & b) { - return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < - 0; + [](const types::DynamicObjects & a, const types::DynamicObjects & b) { + return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().header.stamp); } else { // check time jump back if (now < latest_exported_object_time_) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 64a8c29eecde0..bf706e76f0529 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -29,7 +29,7 @@ namespace autoware::multi_object_tracker { -using ObjectsList = std::vector>; +using ObjectsList = std::vector; struct InputChannel { diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index feec11bfa157a..70a961e3461b0 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -46,7 +46,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index) + const std::unordered_map & direct_assignment) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -55,8 +55,7 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_index); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform, detected_objects.channel_index); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -66,7 +65,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index) + const std::unordered_map & reverse_assignment) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -75,7 +74,7 @@ void TrackerProcessor::spawn( } const auto & new_object = detected_objects.objects.at(i); std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, channel_index); + createNewTracker(new_object, time, self_transform, detected_objects.channel_index); if (tracker) list_tracker_.push_back(tracker); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index fe0663a8687c9..9e7b43942d476 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -56,11 +56,11 @@ class TrackerProcessor void update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index); + const std::unordered_map & direct_assignment); void spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index); + const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); // output From 07df24a532ab1c1b310fc2b5d3bddefc3de76490 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 17:47:54 +0900 Subject: [PATCH 06/23] Revert "chore: add channel index to the DynamicObjects" This reverts commit c7e73f08a8d17b5b085dd330dbf187aabbec6879. Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 10 ++++----- .../lib/object_model/dynamic_object.cpp | 9 ++------ .../src/multi_object_tracker_node.cpp | 17 +++++++------- .../src/multi_object_tracker_node.hpp | 5 ++--- .../src/processor/input_manager.cpp | 22 ++++++++++--------- .../src/processor/input_manager.hpp | 2 +- .../src/processor/processor.cpp | 9 ++++---- .../src/processor/processor.hpp | 4 ++-- 8 files changed, 37 insertions(+), 41 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 9b4887f25cf13..eaac604d41d77 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -31,10 +31,12 @@ #include #include #include +#include #include #include +#include #include #include @@ -83,25 +85,23 @@ struct DynamicObject struct DynamicObjects { std_msgs::msg::Header header; - uint8_t channel_index; std::vector objects; }; DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); -DynamicObjects getDynamicObjects( - const autoware_perception_msgs::msg::DetectedObjects & det_objects); +DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object); } // namespace types + bool transformObjects( const types::DynamicObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); -double getArea(const types::ObjectShape & shape); - + double getArea(const types::ObjectShape & shape); } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 48058b8508f67..9838fa640666f 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -17,12 +17,6 @@ #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" -#include - -#include - -#include - namespace autoware::multi_object_tracker { @@ -113,6 +107,7 @@ autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObjec }; } // namespace types + inline boost::optional getTransform( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) @@ -179,4 +174,4 @@ double getArea(const types::ObjectShape & shape) } return 0.0; }; -} // namespace autoware::multi_object_tracker +} // namespace autoware::multi_object_tracker \ No newline at end of file diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 2ddae0548cd6a..efd9c0952a2d7 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -95,8 +95,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = - create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -240,18 +239,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().header.stamp); + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); // run process for each DynamicObjects for (const auto & objects_data : objects_list) { - runProcess(objects_data); + runProcess(objects_data.second, objects_data.first); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); checkAndPublish(latest_object_time); } } @@ -279,9 +278,9 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) +void MultiObjectTracker::runProcess( + const types::DynamicObjects & input_objects, const uint & channel_index) { - const uint & channel_index = input_objects.channel_index; // Get the time of the measurement const rclcpp::Time measurement_time = rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); @@ -355,14 +354,14 @@ void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment); + processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); } } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 903779905f2c5..5a827889978f8 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -64,8 +64,7 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr - detected_object_sub_; + rclcpp::Subscription::SharedPtr detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -98,7 +97,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const types::DynamicObjects & input_objects); + void runProcess(const types::DynamicObjects & input_objects, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 34edd6e25d677..0f284d97b5c99 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -58,7 +58,6 @@ void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; types::DynamicObjects dynamic_objects = types::getDynamicObjects(objects); - dynamic_objects.channel_index = index_; // Model the object uncertainty only if it is not available types::DynamicObjects objects_with_uncertainty = uncertainty::modelUncertainty(dynamic_objects); @@ -172,7 +171,8 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - objects_list.push_back(objects); + std::pair objects_pair(index_, objects); + objects_list.push_back(objects_pair); } } @@ -220,11 +220,10 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function - func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = - node_.create_subscription( - input_channels[i].input_topic, rclcpp::QoS{1}, func); + std::function func = + std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } // Check if any spawn enabled input streams @@ -344,14 +343,17 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const types::DynamicObjects & a, const types::DynamicObjects & b) { - return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; + []( + const std::pair & a, + const std::pair & b) { + return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < + 0; }); // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_exported_object_time_ = rclcpp::Time(objects_list.back().header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { // check time jump back if (now < latest_exported_object_time_) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index bf706e76f0529..64a8c29eecde0 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -29,7 +29,7 @@ namespace autoware::multi_object_tracker { -using ObjectsList = std::vector; +using ObjectsList = std::vector>; struct InputChannel { diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 70a961e3461b0..feec11bfa157a 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -46,7 +46,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment) + const std::unordered_map & direct_assignment, const uint & channel_index) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -55,7 +55,8 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform, detected_objects.channel_index); + (*(tracker_itr)) + ->updateWithMeasurement(associated_object, time, self_transform, channel_index); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -65,7 +66,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment) + const std::unordered_map & reverse_assignment, const uint & channel_index) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -74,7 +75,7 @@ void TrackerProcessor::spawn( } const auto & new_object = detected_objects.objects.at(i); std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, detected_objects.channel_index); + createNewTracker(new_object, time, self_transform, channel_index); if (tracker) list_tracker_.push_back(tracker); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 9e7b43942d476..fe0663a8687c9 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -56,11 +56,11 @@ class TrackerProcessor void update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment); + const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment); + const std::unordered_map & reverse_assignment, const uint & channel_index); void prune(const rclcpp::Time & time); // output From 210740ee513a6c0b063089bfb9174ccbfcd30043 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 17:50:50 +0900 Subject: [PATCH 07/23] fix: replace trackedobject in the process Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 4 ++- .../tracker/model/bicycle_tracker.hpp | 4 +-- .../model/multiple_vehicle_tracker.hpp | 4 +-- .../tracker/model/pass_through_tracker.hpp | 4 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 4 +-- .../tracker/model/pedestrian_tracker.hpp | 4 +-- .../tracker/model/tracker_base.hpp | 3 +-- .../tracker/model/unknown_tracker.hpp | 4 +-- .../tracker/model/vehicle_tracker.hpp | 4 +-- .../lib/association/association.cpp | 10 +++++--- .../lib/object_model/dynamic_object.cpp | 5 ++-- .../lib/tracker/model/bicycle_tracker.cpp | 4 +-- .../model/multiple_vehicle_tracker.cpp | 2 +- .../tracker/model/pass_through_tracker.cpp | 4 +-- .../model/pedestrian_and_bicycle_tracker.cpp | 2 +- .../lib/tracker/model/pedestrian_tracker.cpp | 4 +-- .../lib/tracker/model/tracker_base.cpp | 2 +- .../lib/tracker/model/unknown_tracker.cpp | 4 +-- .../lib/tracker/model/vehicle_tracker.cpp | 4 +-- .../src/debugger/debug_object.cpp | 2 +- .../src/processor/processor.cpp | 25 +++++++++++-------- 21 files changed, 50 insertions(+), 53 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index eaac604d41d77..8b304d2e79cc3 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include @@ -76,6 +77,7 @@ struct ObjectShape struct DynamicObject { + unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); float_t existence_probability; std::vector classification; ObjectKinematics kinematics; @@ -92,7 +94,7 @@ DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObje DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); -autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object); +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); } // namespace types diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 847520cb05cc9..2cf1da02f2994 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -62,9 +62,7 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: types::DynamicObject getUpdatingObject( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 0cf38a3f38529..92d8949c012b4 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -45,9 +45,7 @@ class MultipleVehicleTracker : public Tracker bool measure( const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index ae34984e65b56..1d66d607392d6 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -44,9 +44,7 @@ class PassThroughTracker : public Tracker bool measure( const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index e0063f66dd2d9..407a61edd236f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -45,9 +45,7 @@ class PedestrianAndBicycleTracker : public Tracker bool measure( const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 664e3125d5f3d..b63a919733cce 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -68,9 +68,7 @@ class PedestrianTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: types::DynamicObject getUpdatingObject( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index e1d5698d9cf68..689983fa2c168 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -112,8 +112,7 @@ class Tracker const geometry_msgs::msg::Transform & self_transform) = 0; public: - virtual bool getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; + virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index b4ab15673a057..c66e3bbbd1089 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -61,9 +61,7 @@ class UnknownTracker : public Tracker const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 09d775eee48cc..508f36e8c9d05 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -64,9 +64,7 @@ class VehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: types::DynamicObject getUpdatingObject( diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 08bb24abddc56..af29774979cb4 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -168,7 +168,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); @@ -186,6 +186,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); const double area = getArea(measurement_object.shape); + // const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate @@ -209,9 +210,12 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const auto meas_obj = types::getTrackedObject(measurement_object); + const auto object1 = types::toTrackedObjectMsg(measurement_object); + const auto object2 = types::toTrackedObjectMsg(tracked_object); const double iou = autoware::object_recognition_utils::get2dIoU( - meas_obj, tracked_object, min_union_iou_area); + object1, object2, min_union_iou_area); + // const double iou = + // shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 9838fa640666f..9865e2faea1b5 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -70,11 +70,12 @@ DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedOb dynamic_objects.objects.emplace_back(getDynamicObject(det_object)); } return dynamic_objects; -}; +} -autoware_perception_msgs::msg::TrackedObject getTrackedObject(const DynamicObject & dyn_object) +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) { autoware_perception_msgs::msg::TrackedObject tracked_object; + tracked_object.object_id = dyn_object.object_id; tracked_object.existence_probability = dyn_object.existence_probability; tracked_object.classification = dyn_object.classification; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 3069c26f4e757..8875cd48b0e8b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -268,9 +268,9 @@ bool BicycleTracker::measure( } bool BicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = types::getTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index f4de5e13d6e23..84c7602d46b12 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -60,7 +60,7 @@ bool MultipleVehicleTracker::measure( } bool MultipleVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 17d5c0c9e26ca..7da69b8288456 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -88,10 +88,10 @@ bool PassThroughTracker::measure( } bool PassThroughTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = types::getTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 343d4631ee3f4..ef1633c0abc0c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -56,7 +56,7 @@ bool PedestrianAndBicycleTracker::measure( } bool PedestrianAndBicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index fc85b21493274..8664d8a571618 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -266,9 +266,9 @@ bool PedestrianTracker::measure( } bool PedestrianTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = types::getTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 2ec995c0e9620..0566620e1f636 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -201,7 +201,7 @@ void Tracker::updateClassification( geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { - autoware_perception_msgs::msg::TrackedObject object; + types::DynamicObject object; getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index e18d300c83331..a60d80c4e0ace 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -213,9 +213,9 @@ bool UnknownTracker::measure( } bool UnknownTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = types::getTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 716452e4fdb1d..b66b844e171aa 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -326,9 +326,9 @@ bool VehicleTracker::measure( } bool VehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = types::getTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index 35734b40ae2e9..f848f77b26b3a 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -95,7 +95,7 @@ void TrackerObjectDebugger::collect( object_data.time = message_time; object_data.channel_id = channel_index; - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); object_data.uuid = uuidToBoostUuid(tracked_object.object_id); object_data.uuid_int = uuidToInt(object_data.uuid); diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index feec11bfa157a..86b997bc12b90 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -14,11 +14,13 @@ #include "processor.hpp" +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include + +#include #include #include @@ -143,12 +145,12 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) { // Iterate through the list of trackers for (auto itr1 = list_tracker_.begin(); itr1 != list_tracker_.end(); ++itr1) { - autoware_perception_msgs::msg::TrackedObject object1; + types::DynamicObject object1; if (!(*itr1)->getTrackedObject(time, object1)) continue; // Compare the current tracker with the remaining trackers for (auto itr2 = std::next(itr1); itr2 != list_tracker_.end(); ++itr2) { - autoware_perception_msgs::msg::TrackedObject object2; + types::DynamicObject object2; if (!(*itr2)->getTrackedObject(time, object2)) continue; // Calculate the distance between the two objects @@ -164,9 +166,12 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) } // Check the Intersection over Union (IoU) between the two objects - const double min_union_iou_area = 1e-2; + constexpr double min_union_iou_area = 1e-2; + const auto obj1 = types::toTrackedObjectMsg(object1); + const auto obj2 = types::toTrackedObjectMsg(object2); const auto iou = - autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + autoware::object_recognition_utils::get2dIoU(obj1, obj2, min_union_iou_area); + // const auto iou = shapes::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; @@ -225,13 +230,13 @@ void TrackerProcessor::getTrackedObjects( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const { tracked_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { // Skip if the tracker is not confident if (!isConfidentTracker(tracker)) continue; // Get the tracked object, extrapolated to the given time - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tracked_objects.objects.push_back(tracked_object); + tracked_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } @@ -241,11 +246,11 @@ void TrackerProcessor::getTentativeObjects( autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const { tentative_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { if (!isConfidentTracker(tracker)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tentative_objects.objects.push_back(tracked_object); + tentative_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } From c5c88fbf592230a3106b7e6826dba92531d38a89 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 17:58:06 +0900 Subject: [PATCH 08/23] fix: Replace transformObjects with shapes::transformObjects for object transformation Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 26 ++--- .../multi_object_tracker/utils/utils.hpp | 2 +- .../lib/association/association.cpp | 11 +- .../lib/object_model/dynamic_object.cpp | 109 +++++++++++------- .../lib/tracker/model/bicycle_tracker.cpp | 6 +- .../lib/tracker/model/pedestrian_tracker.cpp | 16 +-- .../lib/tracker/model/vehicle_tracker.cpp | 6 +- .../src/multi_object_tracker_node.cpp | 2 +- .../src/processor/processor.cpp | 6 +- 9 files changed, 98 insertions(+), 86 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 8b304d2e79cc3..0118a495113dd 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -31,7 +31,6 @@ #include #include #include -#include #include #include @@ -52,12 +51,6 @@ enum OrientationAvailability : uint8_t { AVAILABLE = 2, }; -enum ShapeType : uint8_t { - BOUNDING_BOX = 0, - CYLINDER = 1, - POLYGON = 2, -}; - struct ObjectKinematics { geometry_msgs::msg::PoseWithCovariance pose_with_covariance; @@ -68,20 +61,13 @@ struct ObjectKinematics bool has_twist_covariance = false; }; -struct ObjectShape -{ - ShapeType type; - geometry_msgs::msg::Polygon footprint; - geometry_msgs::msg::Vector3 dimensions; -}; - struct DynamicObject { unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); float_t existence_probability; std::vector classification; ObjectKinematics kinematics; - ObjectShape shape; + autoware_perception_msgs::msg::Shape shape; }; struct DynamicObjects @@ -98,12 +84,18 @@ autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObj } // namespace types - +namespace shapes +{ bool transformObjects( const types::DynamicObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); - double getArea(const types::ObjectShape & shape); + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area = 0.01); +} // namespace shapes + } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index becc6e379ea5e..532d85dbeb1de 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -242,7 +242,7 @@ inline bool convertConvexHullToBoundingBox( output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - output_object.shape.type = types::ShapeType::BOUNDING_BOX; + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; output_object.shape.dimensions.x = max_x - min_x; output_object.shape.dimensions.y = max_y - min_y; output_object.shape.dimensions.z = max_z; diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index af29774979cb4..f4641513ccdb0 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -185,8 +185,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = getArea(measurement_object.shape); - // const double area = autoware::universe_utils::getArea(measurement_object.shape); + const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate @@ -210,12 +209,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const auto object1 = types::toTrackedObjectMsg(measurement_object); - const auto object2 = types::toTrackedObjectMsg(tracked_object); - const double iou = autoware::object_recognition_utils::get2dIoU( - object1, object2, min_union_iou_area); - // const double iou = - // shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); + const double iou = + shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 9865e2faea1b5..2ac3bbc691e4d 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -17,6 +17,19 @@ #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include +#include + +#include + +#include + +#include + +#include +#include +#include + namespace autoware::multi_object_tracker { @@ -48,18 +61,10 @@ DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObje dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; - if (det_object.shape.type == types::ShapeType::BOUNDING_BOX) { - dynamic_object.shape.type = ShapeType::BOUNDING_BOX; - } else if (det_object.shape.type == types::ShapeType::CYLINDER) { - dynamic_object.shape.type = ShapeType::CYLINDER; - } else if (det_object.shape.type == types::ShapeType::POLYGON) { - dynamic_object.shape.type = ShapeType::POLYGON; - } + dynamic_object.shape = det_object.shape; - dynamic_object.shape.footprint = det_object.shape.footprint; - dynamic_object.shape.dimensions = det_object.shape.dimensions; return dynamic_object; -}; +} DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects) { @@ -94,21 +99,14 @@ autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObj } tracked_object.kinematics.is_stationary = false; - if (dyn_object.shape.type == ShapeType::BOUNDING_BOX) { - tracked_object.shape.type = types::ShapeType::BOUNDING_BOX; - } else if (dyn_object.shape.type == ShapeType::CYLINDER) { - tracked_object.shape.type = types::ShapeType::CYLINDER; - } else if (dyn_object.shape.type == ShapeType::POLYGON) { - tracked_object.shape.type = types::ShapeType::POLYGON; - } + tracked_object.shape = dyn_object.shape; - tracked_object.shape.footprint = dyn_object.shape.footprint; - tracked_object.shape.dimensions = dyn_object.shape.dimensions; return tracked_object; -}; +} } // namespace types - +namespace shapes +{ inline boost::optional getTransform( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) @@ -122,7 +120,7 @@ inline boost::optional getTransform( RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); return boost::none; } -}; +} bool transformObjects( const types::DynamicObjects & input_msg, const std::string & target_frame_id, @@ -156,23 +154,54 @@ bool transformObjects( } } return true; -}; +} -double getArea(const types::ObjectShape & shape) +inline double getSumArea(const std::vector & polygons) { - if (shape.type == types::ShapeType::BOUNDING_BOX) { - return shape.dimensions.x * shape.dimensions.y; - } else if (shape.type == types::ShapeType::CYLINDER) { - return M_PI * shape.dimensions.x * shape.dimensions.x * 0.25; - } else if (shape.type == types::ShapeType::POLYGON) { - double area = 0.0; - for (size_t i = 0; i < shape.footprint.points.size(); ++i) { - const auto & p1 = shape.footprint.points.at(i); - const auto & p2 = shape.footprint.points.at((i + 1) % shape.footprint.points.size()); - area += (p1.x * p2.y - p2.x * p1.y) / 2.0; - } - return std::abs(area); - } - return 0.0; -}; -} // namespace autoware::multi_object_tracker \ No newline at end of file + return std::accumulate( + polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area) +{ + static const double MIN_AREA = 1e-6; + + const auto source_polygon = autoware::universe_utils::toPolygon2d( + source_object.kinematics.pose_with_covariance.pose, source_object.shape); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware::universe_utils::toPolygon2d( + target_object.kinematics.pose_with_covariance.pose, target_object.shape); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} +} // namespace shapes +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 8875cd48b0e8b..91844815dd943 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -59,7 +59,7 @@ BicycleTracker::BicycleTracker( initializeExistenceProbabilities(channel_index, object.existence_probability); // OBJECT SHAPE MODEL - if (object.shape.type == types::ShapeType::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { @@ -156,7 +156,7 @@ types::DynamicObject BicycleTracker::getUpdatingObject( // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - if (object.shape.type != types::ShapeType::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } @@ -198,7 +198,7 @@ bool BicycleTracker::measureWithPose(const types::DynamicObject & object) bool BicycleTracker::measureWithShape(const types::DynamicObject & object) { - if (object.shape.type != types::ShapeType::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 8664d8a571618..455e73714c452 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -63,12 +63,12 @@ PedestrianTracker::PedestrianTracker( object_model_.init_size.length, object_model_.init_size.width, object_model_.init_size.height}; // default value cylinder_ = {object_model_.init_size.length, object_model_.init_size.height}; // default value - if (object.shape.type == types::ShapeType::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else if (object.shape.type == types::ShapeType::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; - } else if (object.shape.type == types::ShapeType::POLYGON) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { // do not update polygon shape } // set maximum and minimum size @@ -182,7 +182,7 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; - if (object.shape.type == types::ShapeType::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // check bound box size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] @@ -198,7 +198,7 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - } else if (object.shape.type == types::ShapeType::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // check cylinder size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] @@ -287,15 +287,15 @@ bool PedestrianTracker::getTrackedObject( pose_with_cov.pose.position.z = z_; // set shape - if (object.shape.type == types::ShapeType::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { object.shape.dimensions.x = bounding_box_.length; object.shape.dimensions.y = bounding_box_.width; object.shape.dimensions.z = bounding_box_.height; - } else if (object.shape.type == types::ShapeType::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object.shape.dimensions.x = cylinder_.width; object.shape.dimensions.y = cylinder_.width; object.shape.dimensions.z = cylinder_.height; - } else if (object.shape.type == types::ShapeType::POLYGON) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index b66b844e171aa..d93284d86b628 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -66,7 +66,7 @@ VehicleTracker::VehicleTracker( velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL - if (object.shape.type == types::ShapeType::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { @@ -174,7 +174,7 @@ types::DynamicObject VehicleTracker::getUpdatingObject( // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape types::DynamicObject bbox_object = object; - if (object.shape.type != types::ShapeType::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -242,7 +242,7 @@ bool VehicleTracker::measureWithPose(const types::DynamicObject & object) bool VehicleTracker::measureWithShape(const types::DynamicObject & object) { - if (object.shape.type != types::ShapeType::BOUNDING_BOX) { + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index efd9c0952a2d7..657665fb75721 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -294,7 +294,7 @@ void MultiObjectTracker::runProcess( // Transform the objects to the world frame types::DynamicObjects transformed_objects; - if (!transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { + if (!shapes::transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 86b997bc12b90..e73e9f04afb91 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -167,11 +167,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // Check the Intersection over Union (IoU) between the two objects constexpr double min_union_iou_area = 1e-2; - const auto obj1 = types::toTrackedObjectMsg(object1); - const auto obj2 = types::toTrackedObjectMsg(object2); - const auto iou = - autoware::object_recognition_utils::get2dIoU(obj1, obj2, min_union_iou_area); - // const auto iou = shapes::get2dIoU(object1, object2, min_union_iou_area); + const auto iou = shapes::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; From 95ec16595aea3e514cacae1742464a88b650578e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 18:11:33 +0900 Subject: [PATCH 09/23] chore: add channel index to the DynamicObjects Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 6 ++--- .../lib/uncertainty/uncertainty_processor.cpp | 1 + .../src/multi_object_tracker_node.cpp | 17 +++++++------- .../src/multi_object_tracker_node.hpp | 5 +++-- .../src/processor/input_manager.cpp | 22 +++++++++---------- .../src/processor/input_manager.hpp | 2 +- .../src/processor/processor.cpp | 9 ++++---- .../src/processor/processor.hpp | 4 ++-- 8 files changed, 34 insertions(+), 32 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 0118a495113dd..e7e942a9c355b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -73,12 +73,14 @@ struct DynamicObject struct DynamicObjects { std_msgs::msg::Header header; + uint channel_index; std::vector objects; }; DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); -DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects); +DynamicObjects getDynamicObjects( + const autoware_perception_msgs::msg::DetectedObjects & det_objects); autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); @@ -90,12 +92,10 @@ bool transformObjects( const types::DynamicObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); - double get2dIoU( const types::DynamicObject & source_object, const types::DynamicObject & target_object, const double min_union_area = 0.01); } // namespace shapes - } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index ab1849c00930b..67d082aa93793 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -106,6 +106,7 @@ types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_ob { types::DynamicObjects updating_objects; updating_objects.header = detected_objects.header; + updating_objects.channel_index = detected_objects.channel_index; for (const auto & object : detected_objects.objects) { if (object.kinematics.has_position_covariance) { updating_objects.objects.push_back(object); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 657665fb75721..9a801d4537455 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -95,7 +95,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -239,18 +240,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); // run process for each DynamicObjects for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); + runProcess(objects_data); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); checkAndPublish(latest_object_time); } } @@ -278,9 +279,9 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess( - const types::DynamicObjects & input_objects, const uint & channel_index) +void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) { + const uint & channel_index = input_objects.channel_index; // Get the time of the measurement const rclcpp::Time measurement_time = rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); @@ -354,14 +355,14 @@ void MultiObjectTracker::runProcess( } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + processor_->update(transformed_objects, *self_transform, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + processor_->spawn(transformed_objects, *self_transform, reverse_assignment); } } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 5a827889978f8..903779905f2c5 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -64,7 +64,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -97,7 +98,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const types::DynamicObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjects & input_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 0f284d97b5c99..34edd6e25d677 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -58,6 +58,7 @@ void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; types::DynamicObjects dynamic_objects = types::getDynamicObjects(objects); + dynamic_objects.channel_index = index_; // Model the object uncertainty only if it is not available types::DynamicObjects objects_with_uncertainty = uncertainty::modelUncertainty(dynamic_objects); @@ -171,8 +172,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - std::pair objects_pair(index_, objects); - objects_list.push_back(objects_pair); + objects_list.push_back(objects); } } @@ -220,10 +220,11 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function func = - std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = node_.create_subscription( - input_channels[i].input_topic, rclcpp::QoS{1}, func); + std::function + func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = + node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } // Check if any spawn enabled input streams @@ -343,17 +344,14 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - []( - const std::pair & a, - const std::pair & b) { - return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < - 0; + [](const types::DynamicObjects & a, const types::DynamicObjects & b) { + return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().header.stamp); } else { // check time jump back if (now < latest_exported_object_time_) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 64a8c29eecde0..bf706e76f0529 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -29,7 +29,7 @@ namespace autoware::multi_object_tracker { -using ObjectsList = std::vector>; +using ObjectsList = std::vector; struct InputChannel { diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index e73e9f04afb91..5d6ce3d0f72ef 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -48,7 +48,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index) + const std::unordered_map & direct_assignment) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -58,7 +58,8 @@ void TrackerProcessor::update( const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_index); + ->updateWithMeasurement( + associated_object, time, self_transform, detected_objects.channel_index); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -68,7 +69,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index) + const std::unordered_map & reverse_assignment) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -77,7 +78,7 @@ void TrackerProcessor::spawn( } const auto & new_object = detected_objects.objects.at(i); std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, channel_index); + createNewTracker(new_object, time, self_transform, detected_objects.channel_index); if (tracker) list_tracker_.push_back(tracker); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index fe0663a8687c9..9e7b43942d476 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -56,11 +56,11 @@ class TrackerProcessor void update( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index); + const std::unordered_map & direct_assignment); void spawn( const types::DynamicObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index); + const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); // output From 0914fe149ff72f2d29b850dda65f4bcaf5db8c2a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 18:36:30 +0900 Subject: [PATCH 10/23] feat: separate shape related functions Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 1 + .../object_model/dynamic_object.hpp | 17 +-- .../object_model/shapes.hpp | 41 ++++++ .../lib/association/association.cpp | 4 +- .../lib/object_model/dynamic_object.cpp | 110 -------------- .../lib/object_model/shapes.cpp | 135 ++++++++++++++++++ .../lib/tracker/model/bicycle_tracker.cpp | 10 +- .../tracker/model/pass_through_tracker.cpp | 4 +- .../lib/tracker/model/pedestrian_tracker.cpp | 10 +- .../lib/tracker/model/unknown_tracker.cpp | 10 +- .../lib/tracker/model/vehicle_tracker.cpp | 10 +- .../motion_model/bicycle_motion_model.cpp | 6 +- .../motion_model/ctrv_motion_model.cpp | 6 +- .../tracker/motion_model/cv_motion_model.cpp | 6 +- .../src/multi_object_tracker_node.cpp | 1 + .../src/processor/processor.cpp | 1 + 16 files changed, 214 insertions(+), 158 deletions(-) create mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp create mode 100644 perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index a5526b3326a1b..faa27b0af7379 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -31,6 +31,7 @@ set(${PROJECT_NAME}_lib lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp lib/object_model/dynamic_object.cpp + lib/object_model/shapes.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index e7e942a9c355b..9811ffbedbe3e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -14,7 +14,6 @@ // // // Author: v1.0 Taekjin Lee -// #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ @@ -35,10 +34,6 @@ #include -#include -#include - -#include #include namespace autoware::multi_object_tracker @@ -64,7 +59,7 @@ struct ObjectKinematics struct DynamicObject { unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); - float_t existence_probability; + float existence_probability; std::vector classification; ObjectKinematics kinematics; autoware_perception_msgs::msg::Shape shape; @@ -86,16 +81,6 @@ autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObj } // namespace types -namespace shapes -{ -bool transformObjects( - const types::DynamicObjects & input_msg, const std::string & target_frame_id, - const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); - -double get2dIoU( - const types::DynamicObject & source_object, const types::DynamicObject & target_object, - const double min_union_area = 0.01); -} // namespace shapes } // namespace autoware::multi_object_tracker #endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp new file mode 100644 index 0000000000000..59e86a8e67bb0 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -0,0 +1,41 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ + +#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +bool transformObjects( + const types::DynamicObjects & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area = 0.01); +} // namespace shapes +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index f4641513ccdb0..2781054ecd9fe 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -16,8 +16,10 @@ #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" + +#include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 2ac3bbc691e4d..e1185d1237ac7 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -17,17 +17,6 @@ #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" -#include -#include - -#include - -#include - -#include - -#include -#include #include namespace autoware::multi_object_tracker @@ -105,103 +94,4 @@ autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObj } } // namespace types -namespace shapes -{ -inline boost::optional getTransform( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); - return boost::none; - } -} - -bool transformObjects( - const types::DynamicObjects & input_msg, const std::string & target_frame_id, - const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg) -{ - output_msg = input_msg; - - // transform to world coordinate - if (input_msg.header.frame_id != target_frame_id) { - output_msg.header.frame_id = target_frame_id; - tf2::Transform tf_target2objects_world; - tf2::Transform tf_target2objects; - tf2::Transform tf_objects_world2objects; - { - const auto ros_target2objects_world = - getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); - if (!ros_target2objects_world) { - return false; - } - tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); - } - for (auto & object : output_msg.objects) { - auto & pose_with_cov = object.kinematics.pose_with_covariance; - tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); - tf_target2objects = tf_target2objects_world * tf_objects_world2objects; - // transform pose, frame difference and object pose - tf2::toMsg(tf_target2objects, pose_with_cov.pose); - // transform covariance, only the frame difference - pose_with_cov.covariance = - tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); - } - } - return true; -} - -inline double getSumArea(const std::vector & polygons) -{ - return std::accumulate( - polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { - return acc + boost::geometry::area(p); - }); -} - -inline double getIntersectionArea( - const autoware::universe_utils::Polygon2d & source_polygon, - const autoware::universe_utils::Polygon2d & target_polygon) -{ - std::vector intersection_polygons; - boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); - return getSumArea(intersection_polygons); -} - -inline double getUnionArea( - const autoware::universe_utils::Polygon2d & source_polygon, - const autoware::universe_utils::Polygon2d & target_polygon) -{ - std::vector union_polygons; - boost::geometry::union_(source_polygon, target_polygon, union_polygons); - return getSumArea(union_polygons); -} - -double get2dIoU( - const types::DynamicObject & source_object, const types::DynamicObject & target_object, - const double min_union_area) -{ - static const double MIN_AREA = 1e-6; - - const auto source_polygon = autoware::universe_utils::toPolygon2d( - source_object.kinematics.pose_with_covariance.pose, source_object.shape); - if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d( - target_object.kinematics.pose_with_covariance.pose, target_object.shape); - if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; - - const double intersection_area = getIntersectionArea(source_polygon, target_polygon); - if (intersection_area < MIN_AREA) return 0.0; - const double union_area = getUnionArea(source_polygon, target_polygon); - - const double iou = - union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); - return iou; -} -} // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp new file mode 100644 index 0000000000000..3826b1ee4aa6b --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -0,0 +1,135 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/object_model/shapes.hpp" + +#include +#include + +#include + +#include + +#include + +#include +#include +#include + +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return boost::none; + } +} + +bool transformObjects( + const types::DynamicObjects & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + return true; +} + +inline double getSumArea(const std::vector & polygons) +{ + return std::accumulate( + polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area) +{ + static const double MIN_AREA = 1e-6; + + const auto source_polygon = autoware::universe_utils::toPolygon2d( + source_object.kinematics.pose_with_covariance.pose, source_object.shape); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware::universe_utils::toPolygon2d( + target_object.kinematics.pose_with_covariance.pose, target_object.shape); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} +} // namespace shapes + +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 91844815dd943..a738cb8a0f4a5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -20,14 +20,14 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include +#include +#include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 7da69b8288456..5e32cd4736351 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -19,11 +19,11 @@ #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 455e73714c452..8a00667269969 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -20,14 +20,14 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include +#include +#include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index a60d80c4e0ace..22a19f50ccbca 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -16,13 +16,14 @@ #include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include +#include +#include #include #include @@ -34,7 +35,6 @@ #else #include #endif -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index d93284d86b628..000ad2d3b521c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -20,14 +20,14 @@ #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include +#include +#include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index c65efffe99a47..a72cd59f3940f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -21,12 +21,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 700800e94ecd5..2f5b257eaa8f0 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -21,12 +21,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index e139419197d79..7375a2e60000c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -21,12 +21,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include #include diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 9a801d4537455..3180b9e4ea52a 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,6 +17,7 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 5d6ce3d0f72ef..27783d1ca32e3 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -16,6 +16,7 @@ #include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" #include From 9a406df90b32a53695a40c7d6db6076b1e18b3c4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 18:50:40 +0900 Subject: [PATCH 11/23] chore: clean up utils.hpp Signed-off-by: Taekjin LEE --- .../multi_object_tracker/utils/utils.hpp | 17 ----------------- .../tracker/model/multiple_vehicle_tracker.cpp | 2 +- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 532d85dbeb1de..ec1c84bd579f0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -24,18 +24,12 @@ #include #include -#include #include -#include -#include -#include -#include #include #include #include -#include #include namespace autoware::multi_object_tracker @@ -55,17 +49,6 @@ enum BBOX_IDX { INVALID = -1 }; -/** - * @brief check if object label belongs to "large vehicle" - * @param label: input object label - * @return True if object label means large vehicle - */ -inline bool isLargeVehicleLabel(const uint8_t label) -{ - using Label = autoware_perception_msgs::msg::ObjectClassification; - return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; -} - /** * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle * diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 84c7602d46b12..55c353fe1e659 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -67,7 +67,7 @@ bool MultipleVehicleTracker::getTrackedObject( if (label == Label::CAR) { normal_vehicle_tracker_.getTrackedObject(time, object); - } else if (utils::isLargeVehicleLabel(label)) { + } else if (label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER) { big_vehicle_tracker_.getTrackedObject(time, object); } object.object_id = getUUID(); From 00ab8abdfb132fa5cc7d167b1fb15489096d9472 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 20 Dec 2024 19:19:14 +0900 Subject: [PATCH 12/23] chore: Update function signatures to use DynamicObjectList instead of DynamicObjects Signed-off-by: Taekjin LEE --- .../multi_object_tracker/association/association.hpp | 2 +- .../multi_object_tracker/object_model/dynamic_object.hpp | 4 ++-- .../autoware/multi_object_tracker/object_model/shapes.hpp | 4 ++-- .../uncertainty/uncertainty_processor.hpp | 6 +++--- .../lib/association/association.cpp | 3 ++- .../lib/object_model/dynamic_object.cpp | 5 +++-- .../lib/object_model/shapes.cpp | 4 ++-- .../lib/uncertainty/uncertainty_processor.cpp | 8 ++++---- .../src/debugger/debug_object.cpp | 2 +- .../src/debugger/debug_object.hpp | 2 +- .../src/debugger/debugger.cpp | 2 +- .../src/debugger/debugger.hpp | 2 +- .../src/multi_object_tracker_node.cpp | 6 +++--- .../src/multi_object_tracker_node.hpp | 2 +- .../src/processor/input_manager.cpp | 7 ++++--- .../src/processor/input_manager.hpp | 4 ++-- .../src/processor/processor.cpp | 4 ++-- .../src/processor/processor.hpp | 4 ++-- 18 files changed, 37 insertions(+), 34 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 13eba0c4766d0..b92e17987eb5f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -58,7 +58,7 @@ class DataAssociation const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const types::DynamicObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 9811ffbedbe3e..8c80f66f3dd97 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -65,7 +65,7 @@ struct DynamicObject autoware_perception_msgs::msg::Shape shape; }; -struct DynamicObjects +struct DynamicObjectList { std_msgs::msg::Header header; uint channel_index; @@ -74,7 +74,7 @@ struct DynamicObjects DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); -DynamicObjects getDynamicObjects( +DynamicObjectList getDynamicObjectList( const autoware_perception_msgs::msg::DetectedObjects & det_objects); autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 59e86a8e67bb0..3f7f44d497302 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -29,8 +29,8 @@ namespace autoware::multi_object_tracker namespace shapes { bool transformObjects( - const types::DynamicObjects & input_msg, const std::string & target_frame_id, - const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg); + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg); double get2dIoU( const types::DynamicObject & source_object, const types::DynamicObject & target_object, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index efcd7a1901ba6..8e34ad9884fa0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -38,14 +38,14 @@ using nav_msgs::msg::Odometry; ObjectModel decodeObjectModel(const ObjectClassification & object_class); -types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_objects); +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects); object_model::StateCovariance covarianceFromObjectClass( const types::DynamicObject & detected_object, const ObjectClassification & object_class); -void normalizeUncertainty(types::DynamicObjects & detected_objects); +void normalizeUncertainty(types::DynamicObjectList & detected_objects); -void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjects & detected_objects); +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects); } // namespace uncertainty } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 2781054ecd9fe..49d0e7b011bb9 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -153,7 +153,8 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const types::DynamicObjects & measurements, const std::list> & trackers) + const types::DynamicObjectList & measurements, + const std::list> & trackers) { Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), measurements.objects.size()); diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index e1185d1237ac7..6e7c74ab61c5b 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -55,9 +55,10 @@ DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObje return dynamic_object; } -DynamicObjects getDynamicObjects(const autoware_perception_msgs::msg::DetectedObjects & det_objects) +DynamicObjectList getDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects) { - DynamicObjects dynamic_objects; + DynamicObjectList dynamic_objects; dynamic_objects.header = det_objects.header; dynamic_objects.objects.reserve(det_objects.objects.size()); for (const auto & det_object : det_objects.objects) { diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 3826b1ee4aa6b..21bae6c31e5f8 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -50,8 +50,8 @@ inline boost::optional getTransform( } bool transformObjects( - const types::DynamicObjects & input_msg, const std::string & target_frame_id, - const tf2_ros::Buffer & tf_buffer, types::DynamicObjects & output_msg) + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg) { output_msg = input_msg; diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index 67d082aa93793..36d48d35f74b7 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -102,9 +102,9 @@ types::DynamicObject modelUncertaintyByClass( return updating_object; } -types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_objects) +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects) { - types::DynamicObjects updating_objects; + types::DynamicObjectList updating_objects; updating_objects.header = detected_objects.header; updating_objects.channel_index = detected_objects.channel_index; for (const auto & object : detected_objects.objects) { @@ -119,7 +119,7 @@ types::DynamicObjects modelUncertainty(const types::DynamicObjects & detected_ob return updating_objects; } -void normalizeUncertainty(types::DynamicObjects & detected_objects) +void normalizeUncertainty(types::DynamicObjectList & detected_objects) { constexpr double min_cov_dist = 1e-4; constexpr double min_cov_rad = 1e-6; @@ -140,7 +140,7 @@ void normalizeUncertainty(types::DynamicObjects & detected_objects) } } -void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjects & detected_objects) +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects) { const auto & odom_pose = odometry.pose.pose; const auto & odom_pose_cov = odometry.pose.covariance; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index f848f77b26b3a..dd9ce442a8ab3 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -80,7 +80,7 @@ void TrackerObjectDebugger::reset() void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjects & detected_objects, + const uint & channel_index, const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index 37e1f56281c3e..ed244e0365b28 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,7 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjects & detected_objects, + const uint & channel_index, const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index da956135c32b3..c02d784f01490 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -185,7 +185,7 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjects & detected_objects, + const uint & channel_index, const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 9577eeee1b719..c3b6e67687659 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,7 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjects & detected_objects, + const uint & channel_index, const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 3180b9e4ea52a..b7540109d7df0 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -243,7 +243,7 @@ void MultiObjectTracker::onTrigger() last_updated_time_ = current_time; const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); - // run process for each DynamicObjects + // run process for each DynamicObject for (const auto & objects_data : objects_list) { runProcess(objects_data); } @@ -280,7 +280,7 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) +void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objects) { const uint & channel_index = input_objects.channel_index; // Get the time of the measurement @@ -295,7 +295,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjects & input_objects) } // Transform the objects to the world frame - types::DynamicObjects transformed_objects; + types::DynamicObjectList transformed_objects; if (!shapes::transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 903779905f2c5..33f295e6c81c4 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -98,7 +98,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const types::DynamicObjects & input_objects); + void runProcess(const types::DynamicObjectList & input_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 34edd6e25d677..2902c639d1f58 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -57,11 +57,12 @@ void InputStream::onMessage( { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; - types::DynamicObjects dynamic_objects = types::getDynamicObjects(objects); + types::DynamicObjectList dynamic_objects = types::getDynamicObjectList(objects); dynamic_objects.channel_index = index_; // Model the object uncertainty only if it is not available - types::DynamicObjects objects_with_uncertainty = uncertainty::modelUncertainty(dynamic_objects); + types::DynamicObjectList objects_with_uncertainty = + uncertainty::modelUncertainty(dynamic_objects); // Move the objects_with_uncertainty to the objects queue objects_que_.push_back(std::move(objects_with_uncertainty)); @@ -344,7 +345,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const types::DynamicObjects & a, const types::DynamicObjects & b) { + [](const types::DynamicObjectList & a, const types::DynamicObjectList & b) { return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index bf706e76f0529..9bbed7c85b805 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -29,7 +29,7 @@ namespace autoware::multi_object_tracker { -using ObjectsList = std::vector; +using ObjectsList = std::vector; struct InputChannel { @@ -82,7 +82,7 @@ class InputStream bool is_spawn_enabled_{}; size_t que_size_{30}; - std::deque objects_que_; + std::deque objects_que_; std::function func_trigger_; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 27783d1ca32e3..7960f51272b33 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -47,7 +47,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } void TrackerProcessor::update( - const types::DynamicObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment) { @@ -68,7 +68,7 @@ void TrackerProcessor::update( } void TrackerProcessor::spawn( - const types::DynamicObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment) { diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 9e7b43942d476..890570015b972 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -54,11 +54,11 @@ class TrackerProcessor // tracker processes void predict(const rclcpp::Time & time); void update( - const types::DynamicObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment); void spawn( - const types::DynamicObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); From 82f8941e284dc8fc9743f3474ec030323ac4f84d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 09:19:26 +0900 Subject: [PATCH 13/23] chore: Add channel index to DynamicObject and DynamicObjectList Signed-off-by: Taekjin LEE --- .../object_model/dynamic_object.hpp | 5 +++++ .../lib/object_model/dynamic_object.cpp | 20 ++++++++++++++++--- .../src/multi_object_tracker_node.cpp | 2 +- .../src/processor/input_manager.cpp | 3 +-- 4 files changed, 24 insertions(+), 6 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp index 8c80f66f3dd97..ccd30fb54c55d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp @@ -59,6 +59,7 @@ struct ObjectKinematics struct DynamicObject { unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); + uint channel_index; float existence_probability; std::vector classification; ObjectKinematics kinematics; @@ -72,8 +73,12 @@ struct DynamicObjectList std::vector objects; }; +DynamicObject getDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index); DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); +DynamicObjectList getDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index); DynamicObjectList getDynamicObjectList( const autoware_perception_msgs::msg::DetectedObjects & det_objects); diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 6e7c74ab61c5b..838f1e27430b8 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -25,9 +25,11 @@ namespace autoware::multi_object_tracker namespace types { -DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object) +DynamicObject getDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) { DynamicObject dynamic_object; + dynamic_object.channel_index = channel_index; dynamic_object.existence_probability = det_object.existence_probability; dynamic_object.classification = det_object.classification; @@ -55,18 +57,30 @@ DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObje return dynamic_object; } +DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object) +{ + return getDynamicObject(det_object, 0); +} + DynamicObjectList getDynamicObjectList( - const autoware_perception_msgs::msg::DetectedObjects & det_objects) + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) { DynamicObjectList dynamic_objects; dynamic_objects.header = det_objects.header; + dynamic_objects.channel_index = channel_index; dynamic_objects.objects.reserve(det_objects.objects.size()); for (const auto & det_object : det_objects.objects) { - dynamic_objects.objects.emplace_back(getDynamicObject(det_object)); + dynamic_objects.objects.emplace_back(getDynamicObject(det_object, channel_index)); } return dynamic_objects; } +DynamicObjectList getDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects) +{ + return getDynamicObjectList(det_objects, 0); +} + autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) { autoware_perception_msgs::msg::TrackedObject tracked_object; diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index b7540109d7df0..3118e01ea4969 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -388,7 +388,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 2902c639d1f58..a5235114e190c 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -57,8 +57,7 @@ void InputStream::onMessage( { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; - types::DynamicObjectList dynamic_objects = types::getDynamicObjectList(objects); - dynamic_objects.channel_index = index_; + types::DynamicObjectList dynamic_objects = types::getDynamicObjectList(objects, index_); // Model the object uncertainty only if it is not available types::DynamicObjectList objects_with_uncertainty = From 43456d1aa11c2eae60986832b0969d7fcb1653de Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 09:31:04 +0900 Subject: [PATCH 14/23] chore: Refactor processor and debugger classes to remove channel_index parameter Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 3 +-- .../model/multiple_vehicle_tracker.hpp | 3 +-- .../tracker/model/pass_through_tracker.hpp | 3 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +-- .../tracker/model/pedestrian_tracker.hpp | 3 +-- .../tracker/model/tracker_base.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 3 +-- .../tracker/model/vehicle_tracker.hpp | 2 +- .../lib/tracker/model/bicycle_tracker.cpp | 5 ++-- .../model/multiple_vehicle_tracker.cpp | 9 ++++--- .../tracker/model/pass_through_tracker.cpp | 5 ++-- .../model/pedestrian_and_bicycle_tracker.cpp | 9 ++++--- .../lib/tracker/model/pedestrian_tracker.cpp | 5 ++-- .../lib/tracker/model/tracker_base.cpp | 3 ++- .../lib/tracker/model/unknown_tracker.cpp | 5 ++-- .../lib/tracker/model/vehicle_tracker.cpp | 4 ++-- .../src/debugger/debug_object.cpp | 4 ++-- .../src/debugger/debug_object.hpp | 2 +- .../src/debugger/debugger.cpp | 4 ++-- .../src/debugger/debugger.hpp | 2 +- .../src/multi_object_tracker_node.cpp | 5 ++-- .../src/processor/processor.cpp | 24 +++++++++---------- .../src/processor/processor.hpp | 2 +- 23 files changed, 48 insertions(+), 62 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 2cf1da02f2994..b60de4d369208 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -53,8 +53,7 @@ class BicycleTracker : public Tracker public: BicycleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 92d8949c012b4..efea60d399c02 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -38,8 +38,7 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 1d66d607392d6..c3c16d237abb5 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -38,8 +38,7 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( const types::DynamicObject & object, const rclcpp::Time & time, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 407a61edd236f..71df37e2e4c98 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -38,8 +38,7 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index b63a919733cce..c554b4bab3639 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -59,8 +59,7 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 689983fa2c168..e92a8f4d35337 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -69,7 +69,7 @@ class Tracker } bool updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index c66e3bbbd1089..ebe091d83f860 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -50,8 +50,7 @@ class UnknownTracker : public Tracker public: UnknownTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 508f36e8c9d05..d408d4dcd27bb 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -56,7 +56,7 @@ class VehicleTracker : public Tracker VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, - const size_t channel_size, const uint & channel_index); + const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index a738cb8a0f4a5..0edbe4eb153fb 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -47,8 +47,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +55,7 @@ BicycleTracker::BicycleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 55c353fe1e659..393dc92b1fffa 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -27,16 +27,15 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size) : Tracker(time, object.classification, channel_size), normal_vehicle_tracker_( - object_model::normal_vehicle, time, object, self_transform, channel_size, channel_index), + object_model::normal_vehicle, time, object, self_transform, channel_size), big_vehicle_tracker_( - object_model::big_vehicle, time, object, self_transform, channel_size, channel_index) + object_model::big_vehicle, time, object, self_transform, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 5e32cd4736351..787f3068bd80f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -41,8 +41,7 @@ namespace autoware::multi_object_tracker PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) @@ -51,7 +50,7 @@ PassThroughTracker::PassThroughTracker( prev_observed_object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index ef1633c0abc0c..0fd8b315bb432 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -25,14 +25,13 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size) : Tracker(time, object.classification, channel_size), - pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), - bicycle_tracker_(time, object, self_transform, channel_size, channel_index) + pedestrian_tracker_(time, object, self_transform, channel_size), + bicycle_tracker_(time, object, self_transform, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 8a00667269969..d2aaaaf6a79fe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -47,8 +47,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +55,7 @@ PedestrianTracker::PedestrianTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL bounding_box_ = { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 0566620e1f636..5a4a6588d6983 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -82,7 +82,7 @@ void Tracker::initializeExistenceProbabilities( bool Tracker::updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) + const geometry_msgs::msg::Transform & self_transform) { // Update existence probability { @@ -96,6 +96,7 @@ bool Tracker::updateWithMeasurement( constexpr float probability_false_detection = 0.2; // update measured channel probability without decay + const uint channel_index = object.channel_index; existence_probabilities_[channel_index] = updateProbability( existence_probabilities_[channel_index], probability_true_detection, probability_false_detection); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 22a19f50ccbca..5b5187bf64254 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -41,8 +41,7 @@ namespace autoware::multi_object_tracker UnknownTracker::UnknownTracker( const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -50,7 +49,7 @@ UnknownTracker::UnknownTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // initialize params // measurement noise covariance diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 000ad2d3b521c..e6cae13a08d29 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -48,7 +48,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, - const size_t channel_size, const uint & channel_index) + const size_t channel_size) : Tracker(time, object.classification, channel_size), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), @@ -58,7 +58,7 @@ VehicleTracker::VehicleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // velocity deviation threshold // if the predicted velocity is close to the observed velocity, diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index dd9ce442a8ab3..6a1703e029920 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -80,7 +80,7 @@ void TrackerObjectDebugger::reset() void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjectList & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { @@ -93,13 +93,13 @@ void TrackerObjectDebugger::collect( ++tracker_itr, ++tracker_idx) { ObjectData object_data; object_data.time = message_time; - object_data.channel_id = channel_index; types::DynamicObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); object_data.uuid = uuidToBoostUuid(tracked_object.object_id); object_data.uuid_int = uuidToInt(object_data.uuid); object_data.uuid_str = uuidToString(tracked_object.object_id); + object_data.channel_id = tracked_object.channel_index; // tracker bool is_associated = false; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index ed244e0365b28..5504ee806595f 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,7 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjectList & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index c02d784f01490..4c4beaa582f59 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -185,13 +185,13 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjectList & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( - message_time, list_tracker, channel_index, detected_objects, direct_assignment, + message_time, list_tracker, detected_objects, direct_assignment, reverse_assignment); } diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index c3b6e67687659..3df901a63505c 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,7 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, const types::DynamicObjectList & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 3118e01ea4969..057b0be2cb069 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -282,7 +282,6 @@ void MultiObjectTracker::onTimer() void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objects) { - const uint & channel_index = input_objects.channel_index; // Get the time of the measurement const rclcpp::Time measurement_time = rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); @@ -351,7 +350,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objec // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + measurement_time, processor_->getListTracker(), transformed_objects, direct_assignment, reverse_assignment); } @@ -362,7 +361,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objec processor_->prune(measurement_time); /* spawn new tracker */ - if (input_manager_->isChannelSpawnEnabled(channel_index)) { + if (input_manager_->isChannelSpawnEnabled(input_objects.channel_index)) { processor_->spawn(transformed_objects, *self_transform, reverse_assignment); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 7960f51272b33..b4b16dae392d3 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -60,7 +60,7 @@ void TrackerProcessor::update( detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); (*(tracker_itr)) ->updateWithMeasurement( - associated_object, time, self_transform, detected_objects.channel_index); + associated_object, time, self_transform); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -79,14 +79,14 @@ void TrackerProcessor::spawn( } const auto & new_object = detected_objects.objects.at(i); std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, detected_objects.channel_index); + createNewTracker(new_object, time, self_transform); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const + const geometry_msgs::msg::Transform & self_transform) const { const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); @@ -94,30 +94,28 @@ std::shared_ptr TrackerProcessor::createNewTracker( const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); if (tracker == "big_vehicle_tracker") return std::make_shared( - object_model::big_vehicle, time, object, self_transform, config_.channel_size, - channel_index); + object_model::big_vehicle, time, object, self_transform, config_.channel_size); if (tracker == "multi_vehicle_tracker") return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); if (tracker == "normal_vehicle_tracker") return std::make_shared( - object_model::normal_vehicle, time, object, self_transform, config_.channel_size, - channel_index); + object_model::normal_vehicle, time, object, self_transform, config_.channel_size); if (tracker == "pass_through_tracker") return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); if (tracker == "pedestrian_and_bicycle_tracker") return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); if (tracker == "pedestrian_tracker") return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); } return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + time, object, self_transform, config_.channel_size); } void TrackerProcessor::prune(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 890570015b972..0d393e2a97a20 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -81,7 +81,7 @@ class TrackerProcessor void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; + const geometry_msgs::msg::Transform & self_transform) const; }; } // namespace autoware::multi_object_tracker From 9fc06e091e95103d2679fb39345453fe5a51afe9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 09:32:15 +0900 Subject: [PATCH 15/23] chore: Refactor multiple_vehicle_tracker.cpp and debugger.cpp Signed-off-by: Taekjin LEE --- .../lib/tracker/model/multiple_vehicle_tracker.cpp | 6 ++---- .../src/debugger/debug_object.hpp | 2 +- .../src/debugger/debugger.cpp | 3 +-- .../src/multi_object_tracker_node.cpp | 4 ++-- .../src/processor/processor.cpp | 13 ++++--------- 5 files changed, 10 insertions(+), 18 deletions(-) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 393dc92b1fffa..621e988f32c8b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -29,10 +29,8 @@ MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size) : Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_( - object_model::normal_vehicle, time, object, self_transform, channel_size), - big_vehicle_tracker_( - object_model::big_vehicle, time, object, self_transform, channel_size) + normal_vehicle_tracker_(object_model::normal_vehicle, time, object, self_transform, channel_size), + big_vehicle_tracker_(object_model::big_vehicle, time, object, self_transform, channel_size) { // initialize existence probability initializeExistenceProbabilities(object.channel_index, object.existence_probability); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index 5504ee806595f..d507a0350bbb8 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,7 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const types::DynamicObjectList & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 4c4beaa582f59..3eebb3c11ee3e 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -191,8 +191,7 @@ void TrackerDebugger::collectObjectInfo( { if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( - message_time, list_tracker, detected_objects, direct_assignment, - reverse_assignment); + message_time, list_tracker, detected_objects, direct_assignment, reverse_assignment); } // ObjectDebugger diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 057b0be2cb069..434d7db4c2b39 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -350,8 +350,8 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objec // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), transformed_objects, - direct_assignment, reverse_assignment); + measurement_time, processor_->getListTracker(), transformed_objects, direct_assignment, + reverse_assignment); } /* tracker update */ diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index b4b16dae392d3..538bfb2ca232b 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -58,9 +58,7 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr)) - ->updateWithMeasurement( - associated_object, time, self_transform); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -78,8 +76,7 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform); + std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); if (tracker) list_tracker_.push_back(tracker); } } @@ -93,8 +90,7 @@ std::shared_ptr TrackerProcessor::createNewTracker( if (config_.tracker_map.count(label) != 0) { const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, self_transform, config_.channel_size); if (tracker == "big_vehicle_tracker") return std::make_shared( object_model::big_vehicle, time, object, self_transform, config_.channel_size); @@ -114,8 +110,7 @@ std::shared_ptr TrackerProcessor::createNewTracker( return std::make_shared( time, object, self_transform, config_.channel_size); } - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, self_transform, config_.channel_size); } void TrackerProcessor::prune(const rclcpp::Time & time) From 771dfdc3772096e473064731997b4fbc6f352a3e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 09:51:13 +0900 Subject: [PATCH 16/23] Refactor object tracker classes to remove self_transform parameter Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 3 +-- .../model/multiple_vehicle_tracker.hpp | 3 +-- .../tracker/model/pass_through_tracker.hpp | 3 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +-- .../tracker/model/pedestrian_tracker.hpp | 3 +-- .../tracker/model/unknown_tracker.hpp | 3 +-- .../tracker/model/vehicle_tracker.hpp | 3 +-- .../lib/tracker/model/bicycle_tracker.cpp | 3 +-- .../model/multiple_vehicle_tracker.cpp | 7 +++-- .../tracker/model/pass_through_tracker.cpp | 3 +-- .../model/pedestrian_and_bicycle_tracker.cpp | 7 +++-- .../lib/tracker/model/pedestrian_tracker.cpp | 3 +-- .../lib/tracker/model/unknown_tracker.cpp | 3 +-- .../lib/tracker/model/vehicle_tracker.cpp | 3 +-- .../src/multi_object_tracker_node.cpp | 2 +- .../src/processor/processor.cpp | 26 +++++++------------ .../src/processor/processor.hpp | 4 +-- 17 files changed, 30 insertions(+), 52 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index b60de4d369208..3e61bca66f1e6 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -52,8 +52,7 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index efea60d399c02..1fe2e00582f7f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -37,8 +37,7 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index c3c16d237abb5..dd0d96229ec0f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -37,8 +37,7 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( const types::DynamicObject & object, const rclcpp::Time & time, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 71df37e2e4c98..5b20701e2ea65 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -37,8 +37,7 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c554b4bab3639..506c854a2c2a1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -58,8 +58,7 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index ebe091d83f860..2f97eb9fab51b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -49,8 +49,7 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index d408d4dcd27bb..eafc5fbf5ac44 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -55,8 +55,7 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform, - const size_t channel_size); + const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 0edbe4eb153fb..704f96f0b0c46 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -46,8 +46,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 621e988f32c8b..9f249ab3bc7bc 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -26,11 +26,10 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_(object_model::normal_vehicle, time, object, self_transform, channel_size), - big_vehicle_tracker_(object_model::big_vehicle, time, object, self_transform, channel_size) + normal_vehicle_tracker_(object_model::normal_vehicle, time, object, channel_size), + big_vehicle_tracker_(object_model::big_vehicle, time, object, channel_size) { // initialize existence probability initializeExistenceProbabilities(object.channel_index, object.existence_probability); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 787f3068bd80f..0b7cae8f968ed 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -40,8 +40,7 @@ namespace autoware::multi_object_tracker { PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 0fd8b315bb432..21ce949231062 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -24,11 +24,10 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - pedestrian_tracker_(time, object, self_transform, channel_size), - bicycle_tracker_(time, object, self_transform, channel_size) + pedestrian_tracker_(time, object, channel_size), + bicycle_tracker_(time, object, channel_size) { // initialize existence probability initializeExistenceProbabilities(object.channel_index, object.existence_probability); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index d2aaaaf6a79fe..ba07206322e00 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -46,8 +46,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 5b5187bf64254..28acc2588e6a6 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -40,8 +40,7 @@ namespace autoware::multi_object_tracker { UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index e6cae13a08d29..d5317379f185c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -47,8 +47,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, - const size_t channel_size) + const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 434d7db4c2b39..48a7063505df8 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -362,7 +362,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objec /* spawn new tracker */ if (input_manager_->isChannelSpawnEnabled(input_objects.channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); + processor_->spawn(transformed_objects, reverse_assignment); } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 538bfb2ca232b..5bb487833928f 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -67,7 +67,6 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment) { const auto & time = detected_objects.header.stamp; @@ -76,41 +75,36 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); + std::shared_ptr tracker = createNewTracker(new_object, time); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const + const types::DynamicObject & object, const rclcpp::Time & time) const { const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (config_.tracker_map.count(label) != 0) { const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") - return std::make_shared(time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); if (tracker == "big_vehicle_tracker") return std::make_shared( - object_model::big_vehicle, time, object, self_transform, config_.channel_size); + object_model::big_vehicle, time, object, config_.channel_size); if (tracker == "multi_vehicle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); if (tracker == "normal_vehicle_tracker") return std::make_shared( - object_model::normal_vehicle, time, object, self_transform, config_.channel_size); + object_model::normal_vehicle, time, object, config_.channel_size); if (tracker == "pass_through_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); } - return std::make_shared(time, object, self_transform, config_.channel_size); + return std::make_shared(time, object, config_.channel_size); } void TrackerProcessor::prune(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 0d393e2a97a20..e2ed6e2ee3b75 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -59,7 +59,6 @@ class TrackerProcessor const std::unordered_map & direct_assignment); void spawn( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); @@ -80,8 +79,7 @@ class TrackerProcessor void removeOldTracker(const rclcpp::Time & time); void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const; + const types::DynamicObject & object, const rclcpp::Time & time) const; }; } // namespace autoware::multi_object_tracker From bf1f4abd0ccb0d08df14671f56d2762cd057db0f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 11:07:34 +0900 Subject: [PATCH 17/23] Refactor object tracker classes to use shapes namespace for shape-related functions Signed-off-by: Taekjin LEE --- .../object_model/shapes.hpp | 7 ++ .../multi_object_tracker/utils/utils.hpp | 71 ----------------- .../lib/object_model/shapes.cpp | 76 +++++++++++++++++++ .../lib/tracker/model/bicycle_tracker.cpp | 5 +- .../lib/tracker/model/vehicle_tracker.cpp | 5 +- 5 files changed, 89 insertions(+), 75 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 3f7f44d497302..45c820a83440c 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -35,6 +35,13 @@ bool transformObjects( double get2dIoU( const types::DynamicObject & source_object, const types::DynamicObject & target_object, const double min_union_area = 0.01); + +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object); + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); + } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index ec1c84bd579f0..195f86fc92db6 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -184,77 +184,6 @@ inline void calcAnchorPointOffset( offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); } -/** - * @brief convert convex hull shape object to bounding box object - * @param input_object: input convex hull objects - * @param output_object: output bounding box objects - */ -inline bool convertConvexHullToBoundingBox( - const types::DynamicObject & input_object, types::DynamicObject & output_object) -{ - // check footprint size - if (input_object.shape.footprint.points.size() < 3) { - return false; - } - - // look for bounding box boundary - float max_x = 0; - float max_y = 0; - float min_x = 0; - float min_y = 0; - float max_z = 0; - for (const auto & point : input_object.shape.footprint.points) { - max_x = std::max(max_x, point.x); - max_y = std::max(max_y, point.y); - min_x = std::min(min_x, point.x); - min_y = std::min(min_y, point.y); - max_z = std::max(max_z, point.z); - } - - // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); - const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; - - // set output parameters - output_object = input_object; - output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); - output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = max_x - min_x; - output_object.shape.dimensions.y = max_y - min_y; - output_object.shape.dimensions.z = max_z; - - return true; -} - -inline bool getMeasurementYaw( - const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) -{ - measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - - // check orientation sign is known or not, and fix the limiting delta yaw - double limiting_delta_yaw = M_PI_2; - if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { - limiting_delta_yaw = M_PI; - } - // limiting delta yaw, even the availability is unknown - while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { - if (measurement_yaw < predicted_yaw) { - measurement_yaw += 2 * limiting_delta_yaw; - } else { - measurement_yaw -= 2 * limiting_delta_yaw; - } - } - // return false if the orientation is unknown - return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; -} - } // namespace utils } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 21bae6c31e5f8..5d425651eefc7 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -17,13 +17,17 @@ #include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include +#include #include #include +#include #include #include +#include #include #include @@ -130,6 +134,78 @@ double get2dIoU( union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); return iou; } + +/** + * @brief convert convex hull shape object to bounding box object + * @param input_object: input convex hull objects + * @param output_object: output bounding box objects + */ +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object) +{ + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + + // look for bounding box boundary + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); + } + + // calc new center + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; + + // set output parameters + output_object = input_object; + output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); + output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; + + return true; +} + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; +} + } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 704f96f0b0c46..2b31e193790f2 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -19,6 +19,7 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include @@ -155,7 +156,7 @@ types::DynamicObject BicycleTracker::getUpdatingObject( // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } } @@ -168,7 +169,7 @@ bool BicycleTracker::measureWithPose(const types::DynamicObject & object) // get measurement yaw angle to update const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); + bool is_yaw_available = shapes::getMeasurementYaw(object, tracked_yaw, measurement_yaw); // update bool is_updated = false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index d5317379f185c..6f4fb74bb03d1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -19,6 +19,7 @@ #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include @@ -70,7 +71,7 @@ VehicleTracker::VehicleTracker( object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { types::DynamicObject bbox_object; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::VehicleTracker: Failed to convert convex hull to bounding " @@ -174,7 +175,7 @@ types::DynamicObject VehicleTracker::getUpdatingObject( // convert to bounding box if input is convex shape types::DynamicObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); From 5a000314572c69fa17146639fb3bed52b7ecac87 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 11:26:04 +0900 Subject: [PATCH 18/23] Refactor object tracker classes to use types.hpp for object model types Signed-off-by: Taekjin LEE --- .../autoware/multi_object_tracker/object_model/shapes.hpp | 2 +- .../object_model/{dynamic_object.hpp => types.hpp} | 6 +++--- .../multi_object_tracker/tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/multiple_vehicle_tracker.hpp | 2 +- .../tracker/model/pass_through_tracker.hpp | 2 +- .../tracker/model/pedestrian_and_bicycle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../multi_object_tracker/tracker/model/tracker_base.hpp | 2 +- .../multi_object_tracker/tracker/model/unknown_tracker.hpp | 2 +- .../multi_object_tracker/tracker/model/vehicle_tracker.hpp | 2 +- .../uncertainty/uncertainty_processor.hpp | 2 +- .../include/autoware/multi_object_tracker/utils/utils.hpp | 2 +- .../lib/association/association.cpp | 2 +- .../lib/object_model/dynamic_object.cpp | 2 +- .../src/multi_object_tracker_node.hpp | 2 +- .../src/processor/input_manager.cpp | 2 +- .../src/processor/input_manager.hpp | 2 +- .../src/processor/processor.cpp | 2 +- .../src/processor/processor.hpp | 2 +- 19 files changed, 21 insertions(+), 21 deletions(-) rename perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/{dynamic_object.hpp => types.hpp} (92%) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 45c820a83440c..e4e6e780dc16c 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -18,7 +18,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp similarity index 92% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index ccd30fb54c55d..11660fccb040d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/dynamic_object.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -15,8 +15,8 @@ // // Author: v1.0 Taekjin Lee -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ #include #include @@ -88,4 +88,4 @@ autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObj } // namespace autoware::multi_object_tracker -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__DYNAMIC_OBJECT_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 3e61bca66f1e6..ad3667eb240c0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 1fe2e00582f7f..b9e026bf3c009 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index dd0d96229ec0f..6230ba6e3b0f4 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "tracker_base.hpp" #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 5b20701e2ea65..8a4bfc59d37ac 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 506c854a2c2a1..5a2acc50a8249 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index e92a8f4d35337..d5d8fc74e3baf 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,7 +20,7 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 2f97eb9fab51b..075db6b8a9d69 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index eafc5fbf5ac44..f50d117acc081 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 8e34ad9884fa0..44ad012a5f428 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -18,8 +18,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 195f86fc92db6..5ddc208c22bbd 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 49d0e7b011bb9..9dd37c092a2aa 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -15,8 +15,8 @@ #include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 838f1e27430b8..729ede0c1661b 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -15,7 +15,7 @@ // // Author: v1.0 Taekjin Lee -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 33f295e6c81c4..0472d67617f7f 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -20,7 +20,7 @@ #define MULTI_OBJECT_TRACKER_NODE_HPP_ #include "autoware/multi_object_tracker/association/association.hpp" -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "debugger/debugger.hpp" #include "processor/input_manager.hpp" diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index a5235114e190c..26853aef5555f 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -14,7 +14,7 @@ #include "input_manager.hpp" -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 9bbed7c85b805..189d7b7dc48fe 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -15,7 +15,7 @@ #ifndef PROCESSOR__INPUT_MANAGER_HPP_ #define PROCESSOR__INPUT_MANAGER_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 5bb487833928f..02ad0767dc815 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -14,9 +14,9 @@ #include "processor.hpp" -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" #include diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index e2ed6e2ee3b75..ad296b1c07d8d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -15,7 +15,7 @@ #ifndef PROCESSOR__PROCESSOR_HPP_ #define PROCESSOR__PROCESSOR_HPP_ -#include "autoware/multi_object_tracker/object_model/dynamic_object.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include From bdfaa80c566f7feca1685edfe8c4ac2827908780 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 12:06:44 +0900 Subject: [PATCH 19/23] Refactor object tracker classes to remove unused utils.hpp Signed-off-by: Taekjin LEE --- .../object_model/shapes.hpp | 9 + .../tracker/model/tracker_base.hpp | 1 - .../multi_object_tracker/utils/utils.hpp | 190 ------------------ .../lib/association/association.cpp | 1 - .../lib/object_model/shapes.cpp | 149 +++++++++++++- .../lib/tracker/model/bicycle_tracker.cpp | 1 - .../tracker/model/pass_through_tracker.cpp | 2 - .../lib/tracker/model/pedestrian_tracker.cpp | 2 - .../lib/tracker/model/tracker_base.cpp | 2 - .../lib/tracker/model/unknown_tracker.cpp | 2 - .../lib/tracker/model/vehicle_tracker.cpp | 5 +- .../motion_model/bicycle_motion_model.cpp | 1 - .../motion_model/ctrv_motion_model.cpp | 1 - .../tracker/motion_model/cv_motion_model.cpp | 1 - .../src/multi_object_tracker_node.cpp | 1 - 15 files changed, 159 insertions(+), 209 deletions(-) delete mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index e4e6e780dc16c..d399f356136fc 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -20,6 +20,8 @@ #include "autoware/multi_object_tracker/object_model/types.hpp" +#include + #include #include @@ -42,6 +44,13 @@ bool convertConvexHullToBoundingBox( bool getMeasurementYaw( const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); +int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform); + +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index d5d8fc74e3baf..ac5527fca6400 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -21,7 +21,6 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/object_model/types.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp deleted file mode 100644 index 5ddc208c22bbd..0000000000000 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2020 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. -// -// -// Author: v1.0 Yukihiro Saito -// - -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ - -#include "autoware/multi_object_tracker/object_model/types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include - -namespace autoware::multi_object_tracker -{ -namespace utils -{ -enum BBOX_IDX { - FRONT_SURFACE = 0, - RIGHT_SURFACE = 1, - REAR_SURFACE = 2, - LEFT_SURFACE = 3, - FRONT_R_CORNER = 4, - REAR_R_CORNER = 5, - REAR_L_CORNER = 6, - FRONT_L_CORNER = 7, - INSIDE = 8, - INVALID = -1 -}; - -/** - * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle - * - * @param x: object x coordinate in map frame - * @param y: object y coordinate in map frame - * @param yaw: object yaw orientation in map frame - * @param width: object bounding box width - * @param length: object bounding box length - * @param self_transform: Ego vehicle position in map frame - * @return int index - */ -inline int getNearestCornerOrSurface( - const double x, const double y, const double yaw, const double width, const double length, - const geometry_msgs::msg::Transform & self_transform) -{ - // get local vehicle pose - const double x0 = self_transform.translation.x; - const double y0 = self_transform.translation.y; - - // localize self vehicle pose to object coordinate - // R.T (X0-X) - const double xl = std::cos(yaw) * (x0 - x) + std::sin(yaw) * (y0 - y); - const double yl = -std::sin(yaw) * (x0 - x) + std::cos(yaw) * (y0 - y); - - // Determine Index - // x+ (front) - // __ - // y+ | | y- - // (left) | | (right) - // -- - // x- (rear) - int xgrid = 0; - int ygrid = 0; - const int labels[3][3] = { - {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, - {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, - {BBOX_IDX::REAR_L_CORNER, BBOX_IDX::REAR_SURFACE, BBOX_IDX::REAR_R_CORNER}}; - if (xl > length / 2.0) { - xgrid = 0; // front - } else if (xl > -length / 2.0) { - xgrid = 1; // middle - } else { - xgrid = 2; // rear - } - if (yl > width / 2.0) { - ygrid = 0; // left - } else if (yl > -width / 2.0) { - ygrid = 1; // middle - } else { - ygrid = 2; // right - } - - return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value -} - -/** - * @brief Calc bounding box center offset caused by shape change - * @param dw: width update [m] = w_new - w_old - * @param dl: length update [m] = l_new - l_old - * @param indx: nearest corner index - * @return 2d offset vector caused by shape change - */ -inline Eigen::Vector2d calcOffsetVectorFromShapeChange( - const double dw, const double dl, const int indx) -{ - Eigen::Vector2d offset; - // if surface - if (indx == BBOX_IDX::FRONT_SURFACE) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = 0; - } else if (indx == BBOX_IDX::RIGHT_SURFACE) { - offset(0, 0) = 0; - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_SURFACE) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = 0; - } else if (indx == BBOX_IDX::LEFT_SURFACE) { - offset(0, 0) = 0; - offset(1, 0) = dw / 2.0; // move left - } - // if corner - if (indx == BBOX_IDX::FRONT_R_CORNER) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_R_CORNER) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_L_CORNER) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = dw / 2.0; // move left - } else if (indx == BBOX_IDX::FRONT_L_CORNER) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = dw / 2.0; // move left - } - return offset; // do nothing if indx == INVALID or INSIDE -} - -/** - * @brief Convert input object center to tracking point based on nearest corner information - * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and - * prediction yaw angle - * @param w: last input bounding box width - * @param l: last input bounding box length - * @param indx: last input bounding box closest corner index - * @param input_object: input object bounding box - * @param yaw: current yaw estimation - * @param offset_object: output tracking measurement to feed ekf - * @return nearest corner index(int) - */ -inline void calcAnchorPointOffset( - const double w, const double l, const int indx, const types::DynamicObject & input_object, - const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) -{ - // copy value - offset_object = input_object; - // invalid index - if (indx == BBOX_IDX::INSIDE) { - return; // do nothing - } - - // current object width and height - const double w_n = input_object.shape.dimensions.y; - const double l_n = input_object.shape.dimensions.x; - - // update offset - const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); - tracking_offset = offset; - - // offset input object - const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); - const Eigen::Vector2d rotated_offset = R * tracking_offset; - offset_object.kinematics.pose_with_covariance.pose.position.x += rotated_offset.x(); - offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); -} - -} // namespace utils -} // namespace autoware::multi_object_tracker - -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 9dd37c092a2aa..d74d87489f8db 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -17,7 +17,6 @@ #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/object_model/types.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 5d425651eefc7..fe8b54ff3dbfc 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -17,7 +17,6 @@ #include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include #include #include #include @@ -206,6 +205,154 @@ bool getMeasurementYaw( return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; } +enum BBOX_IDX { + FRONT_SURFACE = 0, + RIGHT_SURFACE = 1, + REAR_SURFACE = 2, + LEFT_SURFACE = 3, + FRONT_R_CORNER = 4, + REAR_R_CORNER = 5, + REAR_L_CORNER = 6, + FRONT_L_CORNER = 7, + INSIDE = 8, + INVALID = -1 +}; + +/** + * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle + * + * @param x: object x coordinate in map frame + * @param y: object y coordinate in map frame + * @param yaw: object yaw orientation in map frame + * @param width: object bounding box width + * @param length: object bounding box length + * @param self_transform: Ego vehicle position in map frame + * @return int index + */ +int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform) +{ + // get local vehicle pose + const double x0 = self_transform.translation.x; + const double y0 = self_transform.translation.y; + + // localize self vehicle pose to object coordinate + // R.T (X0-X) + const double xl = std::cos(yaw) * (x0 - x) + std::sin(yaw) * (y0 - y); + const double yl = -std::sin(yaw) * (x0 - x) + std::cos(yaw) * (y0 - y); + + // Determine Index + // x+ (front) + // __ + // y+ | | y- + // (left) | | (right) + // -- + // x- (rear) + int xgrid = 0; + int ygrid = 0; + const int labels[3][3] = { + {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, + {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, + {BBOX_IDX::REAR_L_CORNER, BBOX_IDX::REAR_SURFACE, BBOX_IDX::REAR_R_CORNER}}; + if (xl > length / 2.0) { + xgrid = 0; // front + } else if (xl > -length / 2.0) { + xgrid = 1; // middle + } else { + xgrid = 2; // rear + } + if (yl > width / 2.0) { + ygrid = 0; // left + } else if (yl > -width / 2.0) { + ygrid = 1; // middle + } else { + ygrid = 2; // right + } + + return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value +} + +/** + * @brief Calc bounding box center offset caused by shape change + * @param dw: width update [m] = w_new - w_old + * @param dl: length update [m] = l_new - l_old + * @param indx: nearest corner index + * @return 2d offset vector caused by shape change + */ +inline Eigen::Vector2d calcOffsetVectorFromShapeChange( + const double dw, const double dl, const int indx) +{ + Eigen::Vector2d offset; + // if surface + if (indx == BBOX_IDX::FRONT_SURFACE) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = 0; + } else if (indx == BBOX_IDX::RIGHT_SURFACE) { + offset(0, 0) = 0; + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_SURFACE) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = 0; + } else if (indx == BBOX_IDX::LEFT_SURFACE) { + offset(0, 0) = 0; + offset(1, 0) = dw / 2.0; // move left + } + // if corner + if (indx == BBOX_IDX::FRONT_R_CORNER) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_R_CORNER) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = -dw / 2.0; // move right + } else if (indx == BBOX_IDX::REAR_L_CORNER) { + offset(0, 0) = -dl / 2.0; // move backward + offset(1, 0) = dw / 2.0; // move left + } else if (indx == BBOX_IDX::FRONT_L_CORNER) { + offset(0, 0) = dl / 2.0; // move forward + offset(1, 0) = dw / 2.0; // move left + } + return offset; // do nothing if indx == INVALID or INSIDE +} + +/** + * @brief Convert input object center to tracking point based on nearest corner information + * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and + * prediction yaw angle + * @param w: last input bounding box width + * @param l: last input bounding box length + * @param indx: last input bounding box closest corner index + * @param input_object: input object bounding box + * @param yaw: current yaw estimation + * @param offset_object: output tracking measurement to feed ekf + * @return nearest corner index(int) + */ +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) +{ + // copy value + offset_object = input_object; + // invalid index + if (indx == BBOX_IDX::INSIDE) { + return; // do nothing + } + + // current object width and height + const double w_n = input_object.shape.dimensions.y; + const double l_n = input_object.shape.dimensions.x; + + // update offset + const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); + tracking_offset = offset; + + // offset input object + const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); + const Eigen::Vector2d rotated_offset = R * tracking_offset; + offset_object.kinematics.pose_with_covariance.pose.position.x += rotated_offset.x(); + offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); +} + } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 2b31e193790f2..e0380a7c33e77 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -20,7 +20,6 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 0b7cae8f968ed..45b3b067e2848 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -18,8 +18,6 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index ba07206322e00..bc53689739439 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -19,8 +19,6 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 5a4a6588d6983..31ad1bf94cadd 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -16,8 +16,6 @@ #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 28acc2588e6a6..ed01165ed8176 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -15,8 +15,6 @@ #include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 6f4fb74bb03d1..749640ce4324a 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -20,7 +20,6 @@ #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -189,9 +188,9 @@ types::DynamicObject VehicleTracker::getUpdatingObject( const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); // get offset measurement - const int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = shapes::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); - utils::calcAnchorPointOffset( + shapes::calcAnchorPointOffset( bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index a72cd59f3940f..1e71bf8548b33 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -20,7 +20,6 @@ #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 2f5b257eaa8f0..e10fbca073047 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -20,7 +20,6 @@ #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 7375a2e60000c..fd3b03da398e1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -20,7 +20,6 @@ #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 48a7063505df8..333b301ce9fcf 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -19,7 +19,6 @@ #include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include From 6619fc81d85c659393c912f6876c065bfcb7f837 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 13:09:26 +0900 Subject: [PATCH 20/23] Refactor object tracker classes to use types.hpp for object model types Signed-off-by: Taekjin LEE --- .../multi_object_tracker/object_model/types.hpp | 7 ++----- .../lib/object_model/dynamic_object.cpp | 11 ----------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index 11660fccb040d..1b07ff0598699 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -74,13 +74,10 @@ struct DynamicObjectList }; DynamicObject getDynamicObject( - const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index); -DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object); + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0); DynamicObjectList getDynamicObjectList( - const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index); -DynamicObjectList getDynamicObjectList( - const autoware_perception_msgs::msg::DetectedObjects & det_objects); + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0); autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp index 729ede0c1661b..894e7602e18f5 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp @@ -57,11 +57,6 @@ DynamicObject getDynamicObject( return dynamic_object; } -DynamicObject getDynamicObject(const autoware_perception_msgs::msg::DetectedObject & det_object) -{ - return getDynamicObject(det_object, 0); -} - DynamicObjectList getDynamicObjectList( const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) { @@ -75,12 +70,6 @@ DynamicObjectList getDynamicObjectList( return dynamic_objects; } -DynamicObjectList getDynamicObjectList( - const autoware_perception_msgs::msg::DetectedObjects & det_objects) -{ - return getDynamicObjectList(det_objects, 0); -} - autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) { autoware_perception_msgs::msg::TrackedObject tracked_object; From 50a4191add78b42ced4459e2789843c33a98c273 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 14:33:16 +0900 Subject: [PATCH 21/23] chore: rename to types.cpp Signed-off-by: Taekjin LEE --- perception/autoware_multi_object_tracker/CMakeLists.txt | 2 +- .../lib/object_model/{dynamic_object.cpp => types.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename perception/autoware_multi_object_tracker/lib/object_model/{dynamic_object.cpp => types.cpp} (100%) diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index faa27b0af7379..fcea50235bf0d 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -30,7 +30,7 @@ set(${PROJECT_NAME}_src set(${PROJECT_NAME}_lib lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp - lib/object_model/dynamic_object.cpp + lib/object_model/types.cpp lib/object_model/shapes.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp diff --git a/perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp similarity index 100% rename from perception/autoware_multi_object_tracker/lib/object_model/dynamic_object.cpp rename to perception/autoware_multi_object_tracker/lib/object_model/types.cpp From 69a0d319128e1451f2cdbcdf2c429149e5250dcb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 14:41:05 +0900 Subject: [PATCH 22/23] rename getDynamicObject to toDynamicObject Signed-off-by: Taekjin LEE --- .../autoware/multi_object_tracker/object_model/types.hpp | 4 ++-- .../lib/object_model/types.cpp | 6 +++--- .../src/processor/input_manager.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index 1b07ff0598699..7dab840aac481 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -73,10 +73,10 @@ struct DynamicObjectList std::vector objects; }; -DynamicObject getDynamicObject( +DynamicObject toDynamicObject( const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0); -DynamicObjectList getDynamicObjectList( +DynamicObjectList toDynamicObjectList( const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0); autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); diff --git a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp index 894e7602e18f5..671d5313d0ff8 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp @@ -25,7 +25,7 @@ namespace autoware::multi_object_tracker namespace types { -DynamicObject getDynamicObject( +DynamicObject toDynamicObject( const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) { DynamicObject dynamic_object; @@ -57,7 +57,7 @@ DynamicObject getDynamicObject( return dynamic_object; } -DynamicObjectList getDynamicObjectList( +DynamicObjectList toDynamicObjectList( const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) { DynamicObjectList dynamic_objects; @@ -65,7 +65,7 @@ DynamicObjectList getDynamicObjectList( dynamic_objects.channel_index = channel_index; dynamic_objects.objects.reserve(det_objects.objects.size()); for (const auto & det_object : det_objects.objects) { - dynamic_objects.objects.emplace_back(getDynamicObject(det_object, channel_index)); + dynamic_objects.objects.emplace_back(toDynamicObject(det_object, channel_index)); } return dynamic_objects; } diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 26853aef5555f..bc461191af271 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -57,7 +57,7 @@ void InputStream::onMessage( { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; - types::DynamicObjectList dynamic_objects = types::getDynamicObjectList(objects, index_); + types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, index_); // Model the object uncertainty only if it is not available types::DynamicObjectList objects_with_uncertainty = From cffd502e621b2532dabd442e2f0cdb63b35010af Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 23 Dec 2024 16:01:30 +0900 Subject: [PATCH 23/23] Update perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp Co-authored-by: Yukihiro Saito --- .../autoware_multi_object_tracker/lib/object_model/shapes.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index fe8b54ff3dbfc..2ce23e5df814e 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -13,7 +13,7 @@ // limitations under the License. // // -// Author: v1.0 Taekjin Lee +// Author: v1.0 Yukihiro Saito, Taekjin Lee #include "autoware/multi_object_tracker/object_model/shapes.hpp"