From 4120833a3b853bff9df42a6f3d6004e7074ddc7e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 27 Jun 2024 15:54:01 +0900 Subject: [PATCH] refactor(radar_object_tracker)!: fix namespace and directory structure (#7658) * chore: refactor radar_object_tracker_node.cpp and related files Signed-off-by: Taekjin LEE * refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE * refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE * refactor: remove MSG_COV_IDX enum in utils.hpp Signed-off-by: Taekjin LEE * refactor: fix the node name Signed-off-by: Taekjin LEE * refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../radar_object_tracker/CMakeLists.txt | 28 +++--- .../association}/data_association.hpp | 26 +++--- .../association}/solver/gnn_solver.hpp | 12 +-- .../solver/gnn_solver_interface.hpp | 10 +- .../association/solver/mu_ssp.hpp} | 12 +-- .../association/solver/ssp.hpp} | 12 +-- .../constant_turn_rate_motion_tracker.hpp | 16 ++-- .../tracker/model/linear_motion_tracker.hpp | 16 ++-- .../tracker/model/tracker_base.hpp | 12 ++- .../tracker/tracker.hpp | 6 +- .../utils/radar_object_tracker_utils.hpp | 23 +++-- .../utils/utils.hpp | 56 ++--------- .../launch/radar_object_tracker.launch.xml | 2 +- perception/radar_object_tracker/package.xml | 1 - .../data_association.cpp | 10 +- .../mu_successive_shortest_path_wrapper.cpp | 6 +- .../ssp}/successive_shortest_path.cpp | 6 +- .../radar_object_tracker_node.cpp | 25 +++-- .../radar_object_tracker_node.hpp | 36 +++---- .../constant_turn_rate_motion_tracker.cpp | 76 ++++++++------- .../tracker/model/linear_motion_tracker.cpp | 93 ++++++++++--------- .../src/tracker/model/tracker_base.cpp | 7 +- .../src/utils/radar_object_tracker_utils.cpp | 6 +- .../radar_object_tracker/src/utils/utils.cpp | 6 +- .../test/test_radar_object_tracker_utils.cpp | 4 +- 25 files changed, 247 insertions(+), 260 deletions(-) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association => autoware_radar_object_tracker/association}/data_association.hpp (78%) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association => autoware_radar_object_tracker/association}/solver/gnn_solver.hpp (55%) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association => autoware_radar_object_tracker/association}/solver/gnn_solver_interface.hpp (71%) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp => autoware_radar_object_tracker/association/solver/mu_ssp.hpp} (69%) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association/solver/successive_shortest_path.hpp => autoware_radar_object_tracker/association/solver/ssp.hpp} (69%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/tracker/model/constant_turn_rate_motion_tracker.hpp (86%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/tracker/model/linear_motion_tracker.hpp (87%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/tracker/model/tracker_base.hpp (89%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/tracker/tracker.hpp (79%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/utils/radar_object_tracker_utils.hpp (81%) rename perception/radar_object_tracker/include/{radar_object_tracker => autoware_radar_object_tracker}/utils/utils.hpp (53%) rename perception/radar_object_tracker/src/{data_association => association}/data_association.cpp (98%) rename perception/radar_object_tracker/src/{data_association/mu_successive_shortest_path => association/mu_ssp}/mu_successive_shortest_path_wrapper.cpp (86%) rename perception/radar_object_tracker/src/{data_association/successive_shortest_path => association/ssp}/successive_shortest_path.cpp (98%) rename perception/radar_object_tracker/src/{radar_object_tracker_node => }/radar_object_tracker_node.cpp (95%) rename perception/radar_object_tracker/{include/radar_object_tracker/radar_object_tracker_node => src}/radar_object_tracker_node.hpp (90%) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index ade45847f8a9f..b732ef6cc863c 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -20,31 +20,31 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(radar_object_tracker_utils SHARED +ament_auto_add_library(${PROJECT_NAME}_utils SHARED src/utils/utils.cpp src/utils/radar_object_tracker_utils.cpp ) -ament_auto_add_library(radar_object_tracker_node SHARED - src/radar_object_tracker_node/radar_object_tracker_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp src/tracker/model/constant_turn_rate_motion_tracker.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/association/data_association.cpp + src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp ) -target_link_libraries(radar_object_tracker_node +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen yaml-cpp nlohmann_json::nlohmann_json # for debug glog::glog - radar_object_tracker_utils + ${PROJECT_NAME}_utils ) -rclcpp_components_register_node(radar_object_tracker_node - PLUGIN "RadarObjectTrackerNode" - EXECUTABLE radar_object_tracker +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_object_tracker::RadarObjectTrackerNode" + EXECUTABLE radar_object_tracker_node ) # testing @@ -52,15 +52,15 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_radar_object_tracker_utils + ament_add_gtest(test_${PROJECT_NAME}_utils test/test_radar_object_tracker_utils.cpp src/utils/radar_object_tracker_utils.cpp ) - target_include_directories(test_radar_object_tracker_utils PRIVATE + target_include_directories(test_${PROJECT_NAME}_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include ) - target_link_libraries(test_radar_object_tracker_utils - radar_object_tracker_utils + target_link_libraries(test_${PROJECT_NAME}_utils + ${PROJECT_NAME}_utils ) endif() diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp similarity index 78% rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp index 434fe66f13790..efde1e6763cdd 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp @@ -16,25 +16,27 @@ // Author: v1.0 Yukihiro Saito // -#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ - -#include -#include -#include -#include +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY + +#include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware_radar_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "radar_object_tracker/data_association/solver/gnn_solver.hpp" -#include "radar_object_tracker/tracker/tracker.hpp" #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include +#include #include +#include +#include +namespace autoware::radar_object_tracker +{ class DataAssociation { private: @@ -62,5 +64,5 @@ class DataAssociation const std::string & file_name); virtual ~DataAssociation() {} }; - -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::radar_object_tracker +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp similarity index 55% rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp index 6d531ac2ee8c2..2aa4a577ef91c 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" -#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" -#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware_radar_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware_radar_object_tracker/association/solver/ssp.hpp" -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 71% rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp index 9b633f04ca190..ee2e98ba47278 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include -namespace gnn_solver +namespace autoware::radar_object_tracker::gnn_solver { class GnnSolverInterface { @@ -30,6 +30,6 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; -} // namespace gnn_solver +} // namespace autoware::radar_object_tracker::gnn_solver -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp similarity index 69% rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp index d4bdd72248670..2a5a336a3eb87 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include -namespace gnn_solver +namespace autoware::radar_object_tracker::gnn_solver { class MuSSP : public GnnSolverInterface { @@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; -} // namespace gnn_solver +} // namespace autoware::radar_object_tracker::gnn_solver -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp similarity index 69% rename from perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp index b08e3c0d302a7..c956e1ebd2f6d 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include -namespace gnn_solver +namespace autoware::radar_object_tracker::gnn_solver { class SSP : public GnnSolverInterface { @@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; -} // namespace gnn_solver +} // namespace autoware::radar_object_tracker::gnn_solver -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp similarity index 86% rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp index 7294f8745bec4..dab4d3f8efa24 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ -#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ -#include "radar_object_tracker/tracker/model/tracker_base.hpp" - -#include +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" +#include "kalman_filter/kalman_filter.hpp" #include - +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker { @@ -108,5 +108,5 @@ class ConstantTurnRateMotionTracker : public Tracker // means constant turn rat autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~ConstantTurnRateMotionTracker() {} }; - -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +} // namespace autoware::radar_object_tracker +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp similarity index 87% rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp index 39b0eaa417c88..7c96aa8fbaa36 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ -#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ -#include "radar_object_tracker/tracker/model/tracker_base.hpp" - -#include +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" +#include "kalman_filter/kalman_filter.hpp" #include +namespace autoware::radar_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; class LinearMotionTracker : public Tracker { @@ -111,5 +113,5 @@ class LinearMotionTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~LinearMotionTracker() {} }; - -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +} // namespace autoware::radar_object_tracker +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp similarity index 89% rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp index 3135aa45b87b4..caea725ef8f81 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp @@ -16,12 +16,12 @@ // Author: v1.0 Yukihiro Saito // -#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY +#include "autoware_radar_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "radar_object_tracker/utils/utils.hpp" #include #include @@ -33,6 +33,8 @@ #include +namespace autoware::radar_object_tracker +{ class Tracker { protected: @@ -92,5 +94,5 @@ class Tracker const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; - -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +} // namespace autoware::radar_object_tracker +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp similarity index 79% rename from perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp index 70045e6b16a07..26222f4c65679 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/constant_turn_rate_motion_tracker.hpp" #include "model/linear_motion_tracker.hpp" #include "model/tracker_base.hpp" -#endif // RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp similarity index 81% rename from perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp index e1c33c022e427..c3aaeb92ee951 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ -#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include +#include +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include #include @@ -33,13 +38,6 @@ #else #include #endif - -#include -#include -#include - -#include - #include #include #include @@ -47,7 +45,8 @@ #include #include -namespace radar_object_tracker_utils + +namespace autoware::radar_object_tracker::utils { boost::optional getTransformAnonymous( @@ -72,6 +71,6 @@ bool hasValidVelocityDirectionToLanelet( const autoware_perception_msgs::msg::TrackedObject & object, const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity); -} // namespace radar_object_tracker_utils +} // namespace autoware::radar_object_tracker::utils -#endif // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp similarity index 53% rename from perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp index 019d684e8bd46..dd011ac274b64 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp @@ -16,16 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include #include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_object.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include #include @@ -33,52 +33,12 @@ #include #include #include - -namespace utils +namespace autoware::radar_object_tracker::utils { -enum MSG_COV_IDX { - X_X = 0, - X_Y = 1, - X_Z = 2, - X_ROLL = 3, - X_PITCH = 4, - X_YAW = 5, - Y_X = 6, - Y_Y = 7, - Y_Z = 8, - Y_ROLL = 9, - Y_PITCH = 10, - Y_YAW = 11, - Z_X = 12, - Z_Y = 13, - Z_Z = 14, - Z_ROLL = 15, - Z_PITCH = 16, - Z_YAW = 17, - ROLL_X = 18, - ROLL_Y = 19, - ROLL_Z = 20, - ROLL_ROLL = 21, - ROLL_PITCH = 22, - ROLL_YAW = 23, - PITCH_X = 24, - PITCH_Y = 25, - PITCH_Z = 26, - PITCH_ROLL = 27, - PITCH_PITCH = 28, - PITCH_YAW = 29, - YAW_X = 30, - YAW_Y = 31, - YAW_Z = 32, - YAW_ROLL = 33, - YAW_PITCH = 34, - YAW_YAW = 35 -}; - // matrix concatenate Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices); Eigen::MatrixXd stackMatricesDiagonally(const std::vector & matrices); -} // namespace utils +} // namespace autoware::radar_object_tracker::utils -#endif // RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ 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 dfebf334456ea..313ef4b0f9fcd 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index c3c4b33946b56..00b9b19113048 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -27,7 +27,6 @@ rclcpp_components tf2 tf2_ros - tier4_perception_msgs unique_identifier_msgs yaml-cpp diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/association/data_association.cpp similarity index 98% rename from perception/radar_object_tracker/src/data_association/data_association.cpp rename to perception/radar_object_tracker/src/association/data_association.cpp index b308f62a24dfa..ac57a3f0a87c0 100644 --- a/perception/radar_object_tracker/src/data_association/data_association.cpp +++ b/perception/radar_object_tracker/src/association/data_association.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/data_association/data_association.hpp" +#include "autoware_radar_object_tracker/association/data_association.hpp" -#include "radar_object_tracker/data_association/solver/gnn_solver.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp" #include -// #include "multi_object_tracker/utils/utils.hpp" - #include #include #include @@ -71,6 +69,8 @@ double getFormedYawAngle( } } // namespace +namespace autoware::radar_object_tracker +{ DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, @@ -294,3 +294,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::radar_object_tracker diff --git a/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp similarity index 86% rename from perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp index 43f974c2621e1..2f47e3b8147d5 100644 --- a/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware_radar_object_tracker/association/solver/mu_ssp.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace gnn_solver +namespace autoware::radar_object_tracker::gnn_solver { void MuSSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, @@ -38,4 +38,4 @@ void MuSSP::maximizeLinearAssignment( // Solve DA by muSSP solve_muSSP(cost, direct_assignment, reverse_assignment); } -} // namespace gnn_solver +} // namespace autoware::radar_object_tracker::gnn_solver diff --git a/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp similarity index 98% rename from perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index c904a9005b27e..7877fe5451d8d 100644 --- a/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware_radar_object_tracker/association/solver/ssp.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace gnn_solver +namespace autoware::radar_object_tracker::gnn_solver { struct ResidualEdge { @@ -367,4 +367,4 @@ void SSP::maximizeLinearAssignment( } #endif } -} // namespace gnn_solver +} // namespace autoware::radar_object_tracker::gnn_solver diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp similarity index 95% rename from perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp rename to perception/radar_object_tracker/src/radar_object_tracker_node.cpp index 7a868b8b11118..10bf11e46abb9 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp" +#define EIGEN_MPL2_ONLY + +#include "radar_object_tracker_node.hpp" -#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" #include #include #include -#include #include @@ -35,8 +36,9 @@ #include #include #include -#define EIGEN_MPL2_ONLY +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options) @@ -140,7 +142,7 @@ void RadarObjectTrackerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) void RadarObjectTrackerNode::onMeasurement( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { - const auto self_transform = radar_object_tracker_utils::getTransformAnonymous( + const auto self_transform = autoware::radar_object_tracker::utils::getTransformAnonymous( tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); if (!self_transform) { return; @@ -236,7 +238,7 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( void RadarObjectTrackerNode::onTimer() { rclcpp::Time current_time = this->now(); - const auto self_transform = radar_object_tracker_utils::getTransformAnonymous( + const auto self_transform = autoware::radar_object_tracker::utils::getTransformAnonymous( tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { return; @@ -283,14 +285,14 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter( for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { autoware_perception_msgs::msg::TrackedObject object; (*itr)->getTrackedObject(time, object); - const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets( + const auto closest_lanelets = autoware::radar_object_tracker::utils::getClosestValidLanelets( object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); // 1. If the object is not close to any lanelet, delete the tracker const bool no_closest_lanelet = closest_lanelets.empty(); // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker const bool is_velocity_direction_close_to_lanelet = - radar_object_tracker_utils::hasValidVelocityDirectionToLanelet( + autoware::radar_object_tracker::utils::hasValidVelocityDirectionToLanelet( object, closest_lanelets, max_lateral_velocity_); if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << @@ -421,5 +423,8 @@ void RadarObjectTrackerNode::publish(const rclcpp::Time & time) const // Publish tracked_objects_pub_->publish(output_msg); } +} // namespace autoware::radar_object_tracker + +#include -RCLCPP_COMPONENTS_REGISTER_NODE(RadarObjectTrackerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_object_tracker::RadarObjectTrackerNode) diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp similarity index 90% rename from perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp rename to perception/radar_object_tracker/src/radar_object_tracker_node.hpp index fe6d472eddd08..dee2cd549c986 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp @@ -12,17 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ -#define RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ +#ifndef RADAR_OBJECT_TRACKER_NODE_HPP_ +#define RADAR_OBJECT_TRACKER_NODE_HPP_ -#include "radar_object_tracker/data_association/data_association.hpp" +#include "autoware_radar_object_tracker/association/data_association.hpp" +#include +#include +#include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +#include +#include +#include +#include +#include +#include #include #include #include @@ -32,17 +41,6 @@ #else #include #endif - -#include -#include -#include - -#include -#include -#include -#include -#include -#include #include #include @@ -51,6 +49,8 @@ #include #include +namespace autoware::radar_object_tracker +{ using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -132,4 +132,6 @@ class RadarObjectTrackerNode : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -#endif // RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ +} // namespace autoware::radar_object_tracker + +#endif // RADAR_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index e1603fbfab702..057a8653faea0 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -16,12 +16,14 @@ // Author: v1.0 Yukihiro Saito // -#include "radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" +#define EIGEN_MPL2_ONLY -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" #include #include @@ -34,7 +36,6 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include #include @@ -42,7 +43,10 @@ #include +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // init static member variables bool ConstantTurnRateMotionTracker::is_initialized_ = false; @@ -114,10 +118,10 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. if (object.kinematics.has_position_covariance) { - P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_xy = R * P_xy_local * R.transpose(); } else { // rotate @@ -128,9 +132,9 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( // covariance often written in object frame if (object.kinematics.has_twist_covariance) { - const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; // const auto vy_cov = - // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + // object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P(IDX::VX, IDX::VX) = vx_cov; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; @@ -371,11 +375,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { - Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance - .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } R_block_list.push_back(Rxy); @@ -427,7 +431,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( if (!object.kinematics.has_twist_covariance) { R_vx << ekf_params_.r_cov_vx; } else { - R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R_vx << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } R_block_list.push_back(R_vx); } @@ -442,9 +446,9 @@ bool ConstantTurnRateMotionTracker::measureWithPose( } // stacking matrices vertically or diagonally - const auto C = utils::stackMatricesVertically(C_list); - const auto Y = utils::stackMatricesVertically(Y_list); - const auto R = utils::stackMatricesDiagonally(R_block_list); + const auto C = autoware::radar_object_tracker::utils::stackMatricesVertically(C_list); + const auto Y = autoware::radar_object_tracker::utils::stackMatricesVertically(Y_list); + const auto R = autoware::radar_object_tracker::utils::stackMatricesDiagonally(R_block_list); // 4. EKF update if (!ekf_.update(Y, C, R)) { @@ -582,27 +586,27 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // twist covariance constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P(IDX::VX, IDX::VX); + twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; // set shape if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -622,3 +626,5 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( return true; } + +} // namespace autoware::radar_object_tracker diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index c6bd93b7a9371..a9f7fffff1156 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -16,12 +16,14 @@ // Author: v1.0 Yukihiro Saito // -#include "radar_object_tracker/tracker/model/linear_motion_tracker.hpp" +#define EIGEN_MPL2_ONLY -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp" -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" #include #include @@ -34,7 +36,6 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include #include @@ -42,7 +43,10 @@ #include +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // initialize static parameter bool LinearMotionTracker::is_initialized_ = false; @@ -119,10 +123,10 @@ LinearMotionTracker::LinearMotionTracker( // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. if (object.kinematics.has_position_covariance) { - P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_xy = R * P_xy_local * R.transpose(); } else { // rotate @@ -130,8 +134,8 @@ LinearMotionTracker::LinearMotionTracker( } // covariance often written in object frame if (object.kinematics.has_twist_covariance) { - const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + const auto vy_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov; P_v_xy = R * P_v_xy_local * R.transpose(); } else { @@ -401,11 +405,11 @@ bool LinearMotionTracker::measureWithPose( Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { - Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance - .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } R_block_list.push_back(Rxy); @@ -433,8 +437,8 @@ bool LinearMotionTracker::measureWithPose( R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); } else { - R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy_local << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X], 0, 0, + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); } R_block_list.push_back(R_v_xy); @@ -453,9 +457,9 @@ bool LinearMotionTracker::measureWithPose( // Eigen::MatrixXd R = Eigen::MatrixXd::Zero(row_number, row_number); // stacking matrices vertically or diagonally - const auto C = utils::stackMatricesVertically(C_list); - const auto Y = utils::stackMatricesVertically(Y_list); - const auto R = utils::stackMatricesDiagonally(R_block_list); + const auto C = autoware::radar_object_tracker::utils::stackMatricesVertically(C_list); + const auto Y = autoware::radar_object_tracker::utils::stackMatricesVertically(Y_list); + const auto R = autoware::radar_object_tracker::utils::stackMatricesDiagonally(R_block_list); // 4. EKF update if (!ekf_.update(Y, C, R)) { @@ -623,37 +627,37 @@ bool LinearMotionTracker::getTrackedObject( constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // twist covariance constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); + twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); + twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; // acceleration covariance - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = no_info_cov; // set shape if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -673,3 +677,4 @@ bool LinearMotionTracker::getTrackedObject( return true; } +} // namespace autoware::radar_object_tracker diff --git a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp index d38b80067cf5a..ce63d96dfb83a 100644 --- a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp @@ -14,13 +14,15 @@ // // -#include "radar_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" #include #include +namespace autoware::radar_object_tracker +{ Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification) @@ -61,3 +63,4 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } +} // namespace autoware::radar_object_tracker diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index ba2500a60beff..f0a6a5dfd384d 100644 --- a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" -namespace radar_object_tracker_utils +namespace autoware::radar_object_tracker::utils { boost::optional getTransformAnonymous( @@ -151,4 +151,4 @@ bool hasValidVelocityDirectionToLanelet( return false; } -} // namespace radar_object_tracker_utils +} // namespace autoware::radar_object_tracker::utils diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/radar_object_tracker/src/utils/utils.cpp index cb9d3813e0ecc..351c4cfd21159 100644 --- a/perception/radar_object_tracker/src/utils/utils.cpp +++ b/perception/radar_object_tracker/src/utils/utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" -namespace utils +namespace autoware::radar_object_tracker::utils { // concatenate matrices vertically Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices) @@ -68,4 +68,4 @@ Eigen::MatrixXd stackMatricesDiagonally(const std::vector & mat return result; } -} // namespace utils +} // namespace autoware::radar_object_tracker::utils diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp index 681ae9f1854f7..e9b2a9800d628 100644 --- a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp +++ b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" #include +using autoware::radar_object_tracker::utils::checkCloseLaneletCondition; using autoware_perception_msgs::msg::TrackedObject; -using radar_object_tracker_utils::checkCloseLaneletCondition; // helper function to create a dummy straight lanelet lanelet::Lanelet createDummyStraightLanelet(double length, double width)