diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index b5d965e1232ed..e2f50b5deea4d 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -10,6 +10,7 @@ This multi object tracker consists of data association and EKF. ![multi_object_tracker_overview](image/multi_object_tracker_overview.svg) + ### Data association The data association performs maximum score matching, called min cost max flow problem. @@ -57,49 +58,24 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Output -| Name | Type | Description | -| ---------- | ----------------------------------------------- | --------------- | -| `~/output` | `autoware_perception_msgs::msg::TrackedObjects` | tracked objects | +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------- | +| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracked objects | ## Parameters ### Input Channel parameters -Available input channels are defined in [input_channels.param.yaml](config/input_channels.param.yaml). - -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ------------------------------------- | -| `` | | the name of channel | -| `.topic` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | -| `.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker | -| `.optional.name` | `std::string` | channel name for analysis | -| `.optional.short_name` | `std::string` | short name for visualization | +{{ json_to_markdown("perception/multi_object_tracker/schema/input_channels.schema.json") }} ### Core Parameters -Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml). - -#### Node parameters - -| Name | Type | Description | -| --------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | -| `***_tracker` | string | EKF tracker name for each class | -| `world_frame_id` | double | object kinematics definition frame | -| `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | -| `publish_rate` | double | Timer frequency to output with delay compensation | -| `publish_processing_time` | bool | enable to publish debug message of process time information | -| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence | -| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection | - -#### Association parameters - -| Name | Type | Description | -| ------------------- | ------ | ------------------------------------------- | -| `can_assign_matrix` | double | Assignment table for data association | -| `max_dist_matrix` | double | Maximum distance table for data association | -| `max_area_matrix` | double | Maximum area table for data association | -| `min_area_matrix` | double | Minimum area table for data association | -| `max_rad_matrix` | double | Maximum angle table for data association | +{{ json_to_markdown("perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} +{{ json_to_markdown("perception/multi_object_tracker/schema/data_association_matrix.schema.json") }} + +#### Simulation parameters + +{{ json_to_markdown("perception/multi_object_tracker/schema/simulation_tracker.schema.json") }} ## Assumptions / Known limits diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 56f4c3ac45e52..a29fcdaa4e1e5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -31,7 +31,7 @@ #include #include -#include +#include class DataAssociation { @@ -55,7 +55,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 autoware_auto_perception_msgs::msg::DetectedObjects & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index c685024d6ef8b..6caa69181dd9d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -21,8 +21,8 @@ #include #include "unique_identifier_msgs/msg/uuid.hpp" -#include -#include +#include +#include #include #include @@ -87,7 +87,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 autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 9ab5576faccd8..1c632f5bebba0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -54,7 +54,7 @@ class TrackerDebugger // ROS node, publishers rclcpp::Node & node_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; @@ -80,7 +80,7 @@ class TrackerDebugger // Object publishing bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } void publishTentativeObjects( - const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const; + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; // Time measurement void startMeasurementTime( @@ -98,7 +98,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 autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 569bc4f43a3d4..3daeaa2a46322 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -27,8 +27,8 @@ #include -#include -#include +#include +#include #include #include @@ -55,9 +55,9 @@ namespace multi_object_tracker { -using DetectedObject = autoware_perception_msgs::msg::DetectedObject; -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; +using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject; +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; class MultiObjectTracker : public rclcpp::Node { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index 29b0c18ae766f..f46c088c6d508 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include #include @@ -28,7 +28,7 @@ namespace multi_object_tracker { -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; using ObjectsList = std::vector>; struct InputChannel @@ -52,7 +52,7 @@ class InputStream func_trigger_ = func_trigger; } - void onMessage(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); bool isTimeInitialized() const { return initial_count_ > 0; } diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index b2f5f41c81f0a..310871dfcb07a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include @@ -39,11 +39,11 @@ class TrackerProcessor // tracker processes void predict(const rclcpp::Time & time); void update( - const autoware_perception_msgs::msg::DetectedObjects & transformed_objects, + const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_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 autoware_auto_perception_msgs::msg::DetectedObjects & 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); @@ -52,10 +52,10 @@ class TrackerProcessor bool isConfidentTracker(const std::shared_ptr & tracker) const; void getTrackedObjects( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const; + autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const; void getTentativeObjects( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const; + autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; void getExistenceProbabilities(std::vector> & existence_vectors) const; @@ -74,7 +74,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 autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 36176b8202e72..ead0d8dbf0e5a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -27,7 +27,7 @@ class BicycleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; private: @@ -55,22 +55,22 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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, + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_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); + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~BicycleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index f284196e1c816..3b509a3cb47d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -27,7 +27,7 @@ class BigVehicleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; private: @@ -60,22 +60,22 @@ class BigVehicleTracker : public Tracker public: BigVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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, + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_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); + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~BigVehicleTracker() {} private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index b3d088da54721..fef8caaf20d8a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -34,17 +34,17 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 4c9a5a113a340..b7810d2471416 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -27,7 +27,7 @@ class NormalVehicleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; private: @@ -60,22 +60,22 @@ class NormalVehicleTracker : public Tracker public: NormalVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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, + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_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); + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~NormalVehicleTracker() {} private: diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 605b3ae462cec..0f892098a373a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -26,23 +26,23 @@ class PassThroughTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; - autoware_perception_msgs::msg::DetectedObject prev_observed_object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject 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 autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~PassThroughTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index ad81c504d3719..490f6d93c6f6a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -33,17 +33,17 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index e5d718c4b15ee..72f5a5a002f0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -29,7 +29,7 @@ class PedestrianTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; private: @@ -63,22 +63,22 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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, + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_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); + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~PedestrianTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 56670e9b54b7e..348ba1f5d7383 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -26,8 +26,8 @@ #include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" +#include "autoware_auto_perception_msgs/msg/tracked_object.hpp" #include "geometry_msgs/msg/point.hpp" #include "unique_identifier_msgs/msg/uuid.hpp" @@ -39,7 +39,7 @@ class Tracker unique_identifier_msgs::msg::UUID uuid_; // classification - std::vector classification_; + std::vector classification_; // existence states int no_measurement_count_; @@ -52,7 +52,7 @@ class Tracker public: Tracker( const rclcpp::Time & time, - const std::vector & classification, + const std::vector & classification, const size_t & channel_size); virtual ~Tracker() {} @@ -64,13 +64,13 @@ class Tracker return existence_vector.size() > 0; } bool updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification - std::vector getClassification() const + std::vector getClassification() const { return classification_; } @@ -91,12 +91,12 @@ class Tracker protected: unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } void setClassification( - const std::vector & classification) + const std::vector & classification) { classification_ = classification; } void updateClassification( - const std::vector & classification); + const std::vector & classification); // virtual functions public: @@ -105,12 +105,13 @@ class Tracker protected: virtual bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; public: virtual bool getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 43a1f2fd14842..3ef58773b408f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -27,7 +27,7 @@ class UnknownTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; private: @@ -48,22 +48,22 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & 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 autoware_auto_perception_msgs::msg::DetectedObject & 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, + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_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); + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; virtual ~UnknownTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 2294462e05fea..5842246bc1313 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -97,7 +97,7 @@ enum BBOX_IDX { */ inline bool isLargeVehicleLabel(const uint8_t label) { - using Label = autoware_perception_msgs::msg::ObjectClassification; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; } @@ -163,11 +163,11 @@ inline int getNearestCornerOrSurface( * @return nearest corner or surface index */ inline int getNearestCornerOrSurfaceFromObject( - const autoware_perception_msgs::msg::DetectedObject & object, const double & yaw, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & yaw, const geometry_msgs::msg::Transform & self_transform) { // only work for BBOX shape - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { return BBOX_IDX::INVALID; } @@ -263,8 +263,9 @@ inline Eigen::Vector2d recoverFromTrackingPoint( */ 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 autoware_auto_perception_msgs::msg::DetectedObject & input_object, const double & yaw, + autoware_auto_perception_msgs::msg::DetectedObject & offset_object, + Eigen::Vector2d & tracking_offset) { // copy value offset_object = input_object; @@ -295,8 +296,8 @@ 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 autoware_auto_perception_msgs::msg::DetectedObject & input_object, + autoware_auto_perception_msgs::msg::DetectedObject & output_object) { // check footprint size if (input_object.shape.footprint.points.size() < 3) { @@ -339,7 +340,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 = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; output_object.shape.dimensions.x = length; output_object.shape.dimensions.y = width; output_object.shape.dimensions.z = height; @@ -348,7 +349,7 @@ inline bool convertConvexHullToBoundingBox( } inline bool getMeasurementYaw( - const autoware_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, double & measurement_yaw) { measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -357,7 +358,7 @@ inline bool getMeasurementYaw( double limiting_delta_yaw = M_PI_2; if ( object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { limiting_delta_yaw = M_PI; } // limiting delta yaw, even the availability is unknown @@ -370,7 +371,7 @@ inline bool getMeasurementYaw( } // return false if the orientation is unknown return object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; } } // namespace utils diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 4e2696700bfdc..1e4a90d8cc08c 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,7 +13,7 @@ autoware_cmake eigen3_cmake_module - autoware_perception_msgs + autoware_auto_perception_msgs diagnostic_updater eigen glog diff --git a/perception/multi_object_tracker/schema/data_association_matrix.schema.json b/perception/multi_object_tracker/schema/data_association_matrix.schema.json new file mode 100644 index 0000000000000..4c9e97164bf20 --- /dev/null +++ b/perception/multi_object_tracker/schema/data_association_matrix.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Data Association Matrix", + "type": "object", + "definitions": { + "data_association_matrix": { + "type": "object", + "properties": { + "can_assign_matrix": { + "type": "array", + "description": "Assignment table for data association.", + "items": { + "type": "integer" + } + }, + "max_dist_matrix": { + "type": "array", + "description": "Maximum distance table for data association.", + "items": { + "type": "number" + } + }, + "max_area_matrix": { + "type": "array", + "description": "Maximum area table for data association.", + "items": { + "type": "number" + } + }, + "min_area_matrix": { + "type": "array", + "description": "Minimum area table for data association.", + "items": { + "type": "number" + } + }, + "max_rad_matrix": { + "type": "array", + "description": "Maximum angle table for data association.", + "items": { + "type": "number" + } + }, + "min_iou_matrix": { + "type": "array", + "description": "A matrix that represents the minimum Intersection over Union (IoU) limit allowed for assignment.", + "items": { + "type": "number" + } + } + }, + "required": [ + "can_assign_matrix", + "max_dist_matrix", + "max_area_matrix", + "min_area_matrix", + "max_rad_matrix", + "min_iou_matrix" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/data_association_matrix" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] + } + \ No newline at end of file diff --git a/perception/multi_object_tracker/schema/input_channels.schema.json b/perception/multi_object_tracker/schema/input_channels.schema.json new file mode 100644 index 0000000000000..3c3da4d3f70a0 --- /dev/null +++ b/perception/multi_object_tracker/schema/input_channels.schema.json @@ -0,0 +1,80 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Input Channels", + "type": "object", + "definitions": { + "input_channel": { + "type": "object", + "properties": { + "topic": { + "type": "string", + "description": "The ROS topic name for the input channel." + }, + "can_spawn_new_tracker": { + "type": "boolean", + "description": "Indicates if the input channel can spawn new trackers." + }, + "optional": { + "type": "object", + "properties": { + "name": { + "type": "string", + "description": "The name of the input channel." + }, + "short_name": { + "type": "string", + "description": "The short name of the input channel." + } + } + } + }, + "required": ["topic", "can_spawn_new_tracker"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "input_channels": { + "type": "object", + "properties": { + "detected_objects": { "$ref": "#/definitions/input_channel" }, + "lidar_clustering": { "$ref": "#/definitions/input_channel" }, + "lidar_centerpoint": { "$ref": "#/definitions/input_channel" }, + "lidar_centerpoint_validated": { "$ref": "#/definitions/input_channel" }, + "lidar_apollo": { "$ref": "#/definitions/input_channel" }, + "lidar_apollo_validated": { "$ref": "#/definitions/input_channel" }, + "lidar_pointpainitng": { "$ref": "#/definitions/input_channel" }, + "lidar_pointpainting_validated": { "$ref": "#/definitions/input_channel" }, + "camera_lidar_fusion": { "$ref": "#/definitions/input_channel" }, + "detection_by_tracker": { "$ref": "#/definitions/input_channel" }, + "radar": { "$ref": "#/definitions/input_channel" }, + "radar_far": { "$ref": "#/definitions/input_channel" } + }, + "required": [ + "detected_objects", + "lidar_clustering", + "lidar_centerpoint", + "lidar_centerpoint_validated", + "lidar_apollo", + "lidar_apollo_validated", + "lidar_pointpainitng", + "lidar_pointpainting_validated", + "camera_lidar_fusion", + "detection_by_tracker", + "radar", + "radar_far" + ] + } + }, + "required": ["input_channels"] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json b/perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json new file mode 100644 index 0000000000000..1a262e6078a80 --- /dev/null +++ b/perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json @@ -0,0 +1,117 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Multi Object Tracker Node", + "type": "object", + "definitions": { + "multi_object_tracker_node": { + "type": "object", + "properties": { + "car_tracker": { + "type": "string", + "description": "Tracker model for car class.", + "default": "multi_vehicle_tracker" + }, + "truck_tracker": { + "type": "string", + "description": "Tracker model for truck class.", + "default": "multi_vehicle_tracker" + }, + "bus_tracker": { + "type": "string", + "description": "Tracker model for bus class.", + "default": "multi_vehicle_tracker" + }, + "trailer_tracker": { + "type": "string", + "description": "Tracker model for trailer class.", + "default": "multi_vehicle_tracker" + }, + "pedestrian_tracker": { + "type": "string", + "description": "Tracker model for pedestrian class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "bicycle_tracker": { + "type": "string", + "description": "Tracker model for bicycle class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "motorcycle_tracker": { + "type": "string", + "description": "Tracker model for motorcycle class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "publish_rate": { + "type": "number", + "description": "Timer frequency to output with delay compensation.", + "default": 10.0 + }, + "world_frame_id": { + "type": "string", + "description": "Object kinematics definition frame.", + "default": "map" + }, + "enable_delay_compensation": { + "type": "boolean", + "description": "If True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp.", + "default": false + }, + "publish_processing_time": { + "type": "boolean", + "description": "Enable to publish debug message of process time information.", + "default": false + }, + "publish_tentative_objects": { + "type": "boolean", + "description": "Enable to publish tentative tracked objects, which have lower confidence.", + "default": false + }, + "publish_debug_markers": { + "type": "boolean", + "description": "Enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection.", + "default": false + }, + "diagnostics_warn_delay": { + "type": "number", + "description": "Delay threshold for warning diagnostics in seconds.", + "default": 0.5 + }, + "diagnostics_error_delay": { + "type": "number", + "description": "Delay threshold for error diagnostics in seconds.", + "default": 1.0 + } + }, + "required": [ + "car_tracker", + "truck_tracker", + "bus_tracker", + "trailer_tracker", + "pedestrian_tracker", + "bicycle_tracker", + "motorcycle_tracker", + "publish_rate", + "world_frame_id", + "enable_delay_compensation", + "publish_processing_time", + "publish_tentative_objects", + "publish_debug_markers", + "diagnostics_warn_delay", + "diagnostics_error_delay" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/multi_object_tracker_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] + } + \ No newline at end of file diff --git a/perception/multi_object_tracker/schema/simulation_tracker.schema.json b/perception/multi_object_tracker/schema/simulation_tracker.schema.json new file mode 100644 index 0000000000000..a78b4d294d7ad --- /dev/null +++ b/perception/multi_object_tracker/schema/simulation_tracker.schema.json @@ -0,0 +1,63 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Simulation Tracker Node", + "type": "object", + "definitions": { + "simulation_tracker_node": { + "type": "object", + "properties": { + "car_tracker": { + "type": "string", + "description": "Tracker model for car class.", + "default": "pass_through_tracker" + }, + "truck_tracker": { + "type": "string", + "description": "Tracker model for truck class.", + "default": "pass_through_tracker" + }, + "bus_tracker": { + "type": "string", + "description": "Tracker model for bus class.", + "default": "pass_through_tracker" + }, + "pedestrian_tracker": { + "type": "string", + "description": "Tracker model for pedestrian class.", + "default": "pass_through_tracker" + }, + "bicycle_tracker": { + "type": "string", + "description": "Tracker model for bicycle class.", + "default": "pass_through_tracker" + }, + "motorcycle_tracker": { + "type": "string", + "description": "Tracker model for motorcycle class.", + "default": "pass_through_tracker" + } + }, + "required": [ + "car_tracker", + "truck_tracker", + "bus_tracker", + "pedestrian_tracker", + "bicycle_tracker", + "motorcycle_tracker" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/simulation_tracker_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] + } + \ No newline at end of file diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 0ff3188e069f2..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -147,7 +147,7 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const autoware_auto_perception_msgs::msg::DetectedObjects & measurements, const std::list> & trackers) { Eigen::MatrixXd score_matrix = @@ -159,14 +159,14 @@ 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 = + const autoware_auto_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index f166e83e136af..ebbeab43a155b 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -14,7 +14,7 @@ #include "multi_object_tracker/debugger/debug_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include "autoware_auto_perception_msgs/msg/tracked_object.hpp" #include @@ -75,7 +75,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 autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { @@ -90,7 +90,7 @@ void TrackerObjectDebugger::collect( object_data.time = message_time; object_data.channel_id = channel_index; - autoware_perception_msgs::msg::TrackedObject tracked_object; + autoware_auto_perception_msgs::msg::TrackedObject 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/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 7b4f9d717a927..afb0ecdc22996 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -29,7 +29,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = - node_.create_publisher( + node_.create_publisher( "~/debug/tentative_objects", rclcpp::QoS{1}); } @@ -83,7 +83,7 @@ void TrackerDebugger::setupDiagnostics() // Object publishing functions void TrackerDebugger::publishTentativeObjects( - const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const { if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_->publish(tentative_objects); @@ -181,7 +181,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 autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 70433f16125c9..54c23d39f5357 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -67,7 +67,7 @@ boost::optional getTransformAnonymous( namespace multi_object_tracker { -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bee76e6c05940..44bdf09046ccd 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -58,7 +58,7 @@ bool InputStream::getTimestamps( } void InputStream::onMessage( - const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { const DetectedObjects objects = *msg; objects_que_.push_back(objects); diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index ff14f7849b161..08df50fa66993 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -17,11 +17,11 @@ #include "multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; TrackerProcessor::TrackerProcessor( const std::map & tracker_map, const size_t & channel_size) @@ -47,7 +47,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } void TrackerProcessor::update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment, const uint & channel_index) { @@ -67,7 +67,7 @@ void TrackerProcessor::update( } void TrackerProcessor::spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & reverse_assignment, const uint & channel_index) { @@ -84,7 +84,7 @@ void TrackerProcessor::spawn( } std::shared_ptr TrackerProcessor::createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); @@ -143,12 +143,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; + autoware_auto_perception_msgs::msg::TrackedObject 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; + autoware_auto_perception_msgs::msg::TrackedObject object2; if (!(*itr2)->getTrackedObject(time, object2)) continue; // Calculate the distance between the two objects @@ -220,14 +220,15 @@ bool TrackerProcessor::isConfidentTracker(const std::shared_ptr & track } void TrackerProcessor::getTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const { tracked_objects.header.stamp = time; 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; + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { tracked_objects.objects.push_back(tracked_object); } @@ -236,12 +237,12 @@ void TrackerProcessor::getTrackedObjects( void TrackerProcessor::getTentativeObjects( const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const + autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const { tentative_objects.header.stamp = time; for (const auto & tracker : list_tracker_) { if (!isConfidentTracker(tracker)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { tentative_objects.objects.push_back(tracked_object); } diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index b54c9b832b0cf..4e5f14fde6834 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -40,10 +40,10 @@ #include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -65,7 +65,7 @@ BicycleTracker::BicycleTracker( ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { @@ -127,9 +127,9 @@ BicycleTracker::BicycleTracker( constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -165,15 +165,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, +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object; + autoware_auto_perception_msgs::msg::DetectedObject updating_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 != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } @@ -195,7 +195,8 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( return updating_object; } -bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { // get measurement yaw angle to update const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); @@ -226,10 +227,11 @@ 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 autoware_auto_perception_msgs::msg::DetectedObject & object) { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } @@ -268,7 +270,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 autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -291,7 +293,7 @@ bool BicycleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -300,7 +302,7 @@ bool BicycleTracker::measure( } bool BicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 5b7a52016967d..15c6b2e722ed5 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -40,10 +40,10 @@ #include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -73,12 +73,12 @@ BigVehicleTracker::BigVehicleTracker( velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -147,9 +147,9 @@ BigVehicleTracker::BigVehicleTracker( constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -188,11 +188,11 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state const double tracked_x = motion_model_.getStateElement(IDX::X); @@ -201,8 +201,8 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -225,7 +225,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // measurement noise covariance float r_cov_x; float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; @@ -243,8 +243,9 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // yaw angle fix double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; @@ -273,7 +274,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje } bool BigVehicleTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -316,9 +317,9 @@ bool BigVehicleTracker::measureWithPose( } bool BigVehicleTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } @@ -362,7 +363,7 @@ bool BigVehicleTracker::measureWithShape( } bool BigVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -385,7 +386,7 @@ bool BigVehicleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -397,7 +398,7 @@ bool BigVehicleTracker::measure( } bool BigVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 67445e5b0daa2..c925976f65e7e 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -18,10 +18,10 @@ #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -40,7 +40,7 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) } bool MultipleVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { big_vehicle_tracker_.measure(object, time, self_transform); @@ -51,9 +51,9 @@ bool MultipleVehicleTracker::measure( } bool MultipleVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - using Label = autoware_perception_msgs::msg::ObjectClassification; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); if (label == Label::CAR) { diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index a4c080cb40eea..bd04cc613ef12 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -40,10 +40,10 @@ #include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -73,12 +73,12 @@ NormalVehicleTracker::NormalVehicleTracker( velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -148,9 +148,9 @@ NormalVehicleTracker::NormalVehicleTracker( constexpr double p0_stddev_y = 0.3; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -189,11 +189,11 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // current (predicted) state const double tracked_x = motion_model_.getStateElement(IDX::X); @@ -202,8 +202,8 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, @@ -227,7 +227,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // measurement noise covariance float r_cov_x; float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (label == Label::CAR) { r_cov_x = ekf_params_.r_cov_x; @@ -245,8 +245,9 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // yaw angle fix double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; @@ -275,7 +276,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO } bool NormalVehicleTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -318,9 +319,9 @@ bool NormalVehicleTracker::measureWithPose( } bool NormalVehicleTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box return false; } @@ -364,7 +365,7 @@ bool NormalVehicleTracker::measureWithShape( } bool NormalVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -387,7 +388,7 @@ bool NormalVehicleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -399,7 +400,7 @@ bool NormalVehicleTracker::measure( } bool NormalVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 3e4f23cd440bc..e1f4383cf11a3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -36,7 +36,7 @@ #include PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -62,7 +62,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) } bool PassThroughTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { prev_observed_object_ = object_; @@ -84,7 +84,7 @@ bool PassThroughTracker::measure( } bool PassThroughTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 00609934990a3..e399dade880fa 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -18,10 +18,10 @@ #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -40,7 +40,7 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { pedestrian_tracker_.measure(object, time, self_transform); @@ -51,9 +51,9 @@ bool PedestrianAndBicycleTracker::measure( } bool PedestrianAndBicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { - using Label = autoware_perception_msgs::msg::ObjectClassification; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); if (label == Label::PEDESTRIAN) { diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 6c8198b51ecd0..fd9ef82dfdca3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -40,10 +40,10 @@ #include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -67,12 +67,12 @@ PedestrianTracker::PedestrianTracker( // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; cylinder_ = {0.5, 1.7}; - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (object.shape.type == autoware_auto_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 == autoware_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::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 == autoware_auto_perception_msgs::msg::Shape::POLYGON) { // do not update polygon shape } // set maximum and minimum size @@ -122,9 +122,9 @@ PedestrianTracker::PedestrianTracker( constexpr double p0_stddev_y = 2.0; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); @@ -160,11 +160,11 @@ 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, +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -186,7 +186,7 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje } bool PedestrianTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object) { // update motion model bool is_updated = false; @@ -207,12 +207,12 @@ bool PedestrianTracker::measureWithPose( } bool PedestrianTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & 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 == autoware_auto_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] @@ -229,7 +229,7 @@ bool PedestrianTracker::measureWithShape( bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; 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 == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { // check cylinder size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] @@ -258,7 +258,7 @@ bool PedestrianTracker::measureWithShape( } bool PedestrianTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -280,7 +280,7 @@ bool PedestrianTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -290,7 +290,7 @@ bool PedestrianTracker::measure( } bool PedestrianTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); @@ -311,15 +311,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 == autoware_auto_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 == autoware_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_auto_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 == autoware_perception_msgs::msg::Shape::POLYGON) { + } else if (object.shape.type == autoware_auto_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/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 6d2787ac1d550..4318da0569721 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -31,7 +31,7 @@ float updateProbability(float prior, float true_positive, float false_positive) Tracker::Tracker( const rclcpp::Time & time, - const std::vector & classification, + const std::vector & classification, const size_t & channel_size) : classification_(classification), no_measurement_count_(0), @@ -62,7 +62,7 @@ void Tracker::initializeExistenceProbabilities( } bool Tracker::updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) { @@ -122,7 +122,7 @@ bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) } void Tracker::updateClassification( - const std::vector & classification) + const std::vector & classification) { // classification algorithm: // 0. Normalize the input classification @@ -139,7 +139,7 @@ void Tracker::updateClassification( // Normalization function auto normalizeProbabilities = - [](std::vector & classification) { + [](std::vector & classification) { double sum = 0.0; for (const auto & class_ : classification) { sum += class_.probability; @@ -185,7 +185,7 @@ void Tracker::updateClassification( geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { - autoware_perception_msgs::msg::TrackedObject object; + autoware_auto_perception_msgs::msg::TrackedObject object; getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index f15d5042a2316..04638267f7ad8 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -37,7 +37,7 @@ #include UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), @@ -140,11 +140,11 @@ 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, +autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -165,7 +165,8 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( return updating_object; } -bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool UnknownTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { // update motion model bool is_updated = false; @@ -186,7 +187,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 autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -203,7 +204,7 @@ bool UnknownTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); @@ -211,7 +212,7 @@ bool UnknownTracker::measure( } bool UnknownTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const { object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID();