diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 07b53fb671732..85c6e87c0fd19 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,8 +1,6 @@ - - @@ -19,8 +17,7 @@ - - + @@ -29,15 +26,12 @@ - - + - - diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 96ad80131e4e7..da393b1efff59 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,6 +14,7 @@ + diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 0cc8f1708e334..311fe7b6da716 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -70,16 +70,26 @@ Example: ### Core 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 | -| `world_frame_id` | double | tracking frame | -| `enable_delay_compensation` | bool | Estimate obstacles at current time considering detection delay | -| `publish_rate` | double | if enable_delay_compensation is true, how many hertz to output | +Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.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 | + +#### 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 | ## Assumptions / Known limits diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml similarity index 68% rename from perception/multi_object_tracker/config/default_tracker.param.yaml rename to perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index 23e88d8274172..9552b35b65bed 100644 --- a/perception/multi_object_tracker/config/default_tracker.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + # default tracker models for each class car_tracker: "multi_vehicle_tracker" truck_tracker: "multi_vehicle_tracker" bus_tracker: "multi_vehicle_tracker" @@ -7,3 +8,8 @@ pedestrian_tracker: "pedestrian_and_bicycle_tracker" bicycle_tracker: "pedestrian_and_bicycle_tracker" motorcycle_tracker: "pedestrian_and_bicycle_tracker" + + # default tracker node parameters + publish_rate: 10.0 + world_frame_id: map + enable_delay_compensation: false diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 21a2ce67d552d..dcd080b851c20 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -2,18 +2,12 @@ - - - - + - - - 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 d302b04d733cd..e42ff748c503e 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -78,9 +78,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) create_publisher("output", rclcpp::QoS{1}); // Parameters - double publish_rate = declare_parameter("publish_rate", 30.0); - world_frame_id_ = declare_parameter("world_frame_id", "world"); - bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; + double publish_rate = declare_parameter("publish_rate"); + world_frame_id_ = declare_parameter("world_frame_id"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 6e6813d3e40ff..60ed2efb5767b 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -2,9 +2,6 @@ - - - @@ -19,8 +16,5 @@ - - -