From 8ef517301327ec73cd4881d0827b90c31b2410aa Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 17:07:29 +0900 Subject: [PATCH 1/6] chore: refactor radar_object_tracker_node.cpp and related files Signed-off-by: Taekjin LEE --- .../radar_object_tracker/CMakeLists.txt | 22 +++++++++---------- .../radar_object_tracker_node.hpp | 6 ++--- .../launch/radar_object_tracker.launch.xml | 2 +- .../radar_object_tracker_node.cpp | 5 +++-- 4 files changed, 18 insertions(+), 17 deletions(-) rename perception/radar_object_tracker/include/radar_object_tracker/{radar_object_tracker_node => }/radar_object_tracker_node.hpp (94%) rename perception/radar_object_tracker/src/{radar_object_tracker_node => }/radar_object_tracker_node.cpp (99%) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index ade45847f8a9f..b4e8f2ce264bd 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -20,13 +20,13 @@ 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 @@ -34,17 +34,17 @@ ament_auto_add_library(radar_object_tracker_node SHARED src/data_association/mu_successive_shortest_path/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 +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "RadarObjectTrackerNode" - EXECUTABLE radar_object_tracker + EXECUTABLE ${PROJECT_NAME}_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/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node.hpp similarity index 94% 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/include/radar_object_tracker/radar_object_tracker_node.hpp index fe6d472eddd08..aabf6d42d752b 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node.hpp @@ -12,8 +12,8 @@ // 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__RADAR_OBJECT_TRACKER_NODE_HPP_ +#define RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ #include "radar_object_tracker/data_association/data_association.hpp" @@ -132,4 +132,4 @@ 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_ +#endif // RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_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/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp similarity index 99% 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..488a9f4df91dc 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,7 +12,9 @@ // 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/radar_object_tracker_node.hpp" #include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" #include "radar_object_tracker/utils/utils.hpp" @@ -35,7 +37,6 @@ #include #include #include -#define EIGEN_MPL2_ONLY using Label = autoware_perception_msgs::msg::ObjectClassification; From 488524890f65d10a6850639b09b971530d322dce Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 17:26:27 +0900 Subject: [PATCH 2/6] refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE --- .../radar_object_tracker/CMakeLists.txt | 4 +-- .../association}/data_association.hpp | 23 ++++++------- .../association}/solver/gnn_solver.hpp | 12 +++---- .../solver/gnn_solver_interface.hpp | 6 ++-- .../association/solver/mu_ssp.hpp} | 8 ++--- .../association/solver/ssp.hpp} | 8 ++--- .../radar_object_tracker_node.hpp | 32 +++++++++---------- .../constant_turn_rate_motion_tracker.hpp | 11 +++---- .../tracker/model/linear_motion_tracker.hpp | 11 +++---- .../tracker/model/tracker_base.hpp | 8 ++--- .../radar_object_tracker/tracker/tracker.hpp | 6 ++-- .../utils/radar_object_tracker_utils.hpp | 19 ++++++----- .../radar_object_tracker/utils/utils.hpp | 12 +++---- .../data_association.cpp | 6 ++-- .../mu_successive_shortest_path_wrapper.cpp | 2 +- .../ssp}/successive_shortest_path.cpp | 2 +- .../src/radar_object_tracker_node.cpp | 6 ++-- .../constant_turn_rate_motion_tracker.cpp | 10 +++--- .../tracker/model/linear_motion_tracker.cpp | 7 ++-- .../src/tracker/model/tracker_base.cpp | 4 +-- .../src/utils/radar_object_tracker_utils.cpp | 2 +- .../radar_object_tracker/src/utils/utils.cpp | 2 +- .../test/test_radar_object_tracker_utils.cpp | 2 +- 23 files changed, 99 insertions(+), 104 deletions(-) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association => autoware/radar_object_tracker/association}/data_association.hpp (81%) 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 (79%) 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} (73%) rename perception/radar_object_tracker/include/{radar_object_tracker/data_association/solver/successive_shortest_path.hpp => autoware/radar_object_tracker/association/solver/ssp.hpp} (74%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/radar_object_tracker_node.hpp (91%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp (88%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/tracker/model/linear_motion_tracker.hpp (89%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/tracker/model/tracker_base.hpp (91%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/tracker/tracker.hpp (79%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/utils/radar_object_tracker_utils.hpp (85%) rename perception/radar_object_tracker/include/{ => autoware}/radar_object_tracker/utils/utils.hpp (82%) 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 (93%) rename perception/radar_object_tracker/src/{data_association/successive_shortest_path => association/ssp}/successive_shortest_path.cpp (99%) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index b4e8f2ce264bd..813f35bad0789 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -30,8 +30,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED 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(${PROJECT_NAME} 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 81% 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..3b6b2bcb9eb19 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,26 @@ // 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 + class DataAssociation { private: @@ -63,4 +64,4 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#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..d009b1e3ac60a 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 79% 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..587ea6692da87 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,8 +12,8 @@ // 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 @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace 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 73% 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..4567f1f50a439 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,10 +12,10 @@ // 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 @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace 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 74% 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..61a2bfa21dfef 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,10 +12,10 @@ // 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 @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace 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/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp similarity index 91% rename from perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node.hpp rename to perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp index aabf6d42d752b..b69d88c14bd53 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/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_HPP_ -#define RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ +#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ +#define AUTOWARE__RADAR_OBJECT_TRACKER__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 @@ -132,4 +130,4 @@ class RadarObjectTrackerNode : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -#endif // RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ +#endif // AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_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 88% 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..7bc262672e74d 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,12 +12,11 @@ // 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 @@ -109,4 +108,4 @@ class ConstantTurnRateMotionTracker : public Tracker // means constant turn rat virtual ~ConstantTurnRateMotionTracker() {} }; -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#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 89% 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..35a1fed3409d1 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,12 +12,11 @@ // 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 @@ -112,4 +111,4 @@ class LinearMotionTracker : public Tracker virtual ~LinearMotionTracker() {} }; -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#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 91% 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..96aa9ac7c08d2 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 @@ -93,4 +93,4 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; -#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#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..06dfec94bf83c 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 85% 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..743db51188fed 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,6 +45,7 @@ #include #include + namespace radar_object_tracker_utils { @@ -74,4 +73,4 @@ bool hasValidVelocityDirectionToLanelet( } // namespace 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 82% 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..b4c2b9cdfc470 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 @@ -81,4 +81,4 @@ Eigen::MatrixXd stackMatricesDiagonally(const std::vector & mat } // namespace utils -#endif // RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ 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..78f2aaaa6b349 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 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 93% 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..cb8a2967208c5 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 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 99% 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..a5cfa65a4ca2d 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 diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp index 488a9f4df91dc..cbe8183500fcc 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp @@ -14,10 +14,10 @@ #define EIGEN_MPL2_ONLY -#include "radar_object_tracker/radar_object_tracker_node.hpp" +#include "autoware/radar_object_tracker/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 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..176ca2ca34465 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,13 @@ // 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/radar_object_tracker/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -34,7 +35,6 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include #include 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 1bc548fa7a951..a693c584a96a3 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,9 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#include "radar_object_tracker/tracker/model/linear_motion_tracker.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/radar_object_tracker/tracker/model/linear_motion_tracker.hpp" -#include "radar_object_tracker/utils/utils.hpp" +#include "autoware/radar_object_tracker/utils/utils.hpp" #include #include @@ -34,7 +36,6 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include #include 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..15742c5a191e7 100644 --- a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp @@ -14,9 +14,9 @@ // // -#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 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..5fa3c91e3a3d9 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,7 +12,7 @@ // 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 { diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/radar_object_tracker/src/utils/utils.cpp index cb9d3813e0ecc..199a32ee0c9fa 100644 --- a/perception/radar_object_tracker/src/utils/utils.cpp +++ b/perception/radar_object_tracker/src/utils/utils.cpp @@ -12,7 +12,7 @@ // 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 { 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..9f35e31641638 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,7 +12,7 @@ // 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 From ee204c039c87b9dfeff0667d917c5c7fd044714f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 17:42:49 +0900 Subject: [PATCH 3/6] refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE --- perception/radar_object_tracker/CMakeLists.txt | 2 +- .../association/data_association.hpp | 5 +++-- .../association/solver/gnn_solver_interface.hpp | 4 ++-- .../association/solver/mu_ssp.hpp | 4 ++-- .../association/solver/ssp.hpp | 4 ++-- .../radar_object_tracker_node.hpp | 4 ++++ .../model/constant_turn_rate_motion_tracker.hpp | 5 +++-- .../tracker/model/linear_motion_tracker.hpp | 5 ++++- .../tracker/model/tracker_base.hpp | 4 +++- .../utils/radar_object_tracker_utils.hpp | 4 ++-- .../radar_object_tracker/utils/utils.hpp | 5 ++--- .../src/association/data_association.cpp | 4 ++++ .../mu_successive_shortest_path_wrapper.cpp | 4 ++-- .../association/ssp/successive_shortest_path.cpp | 4 ++-- .../src/radar_object_tracker_node.cpp | 16 ++++++++++------ .../model/constant_turn_rate_motion_tracker.cpp | 10 +++++++--- .../src/tracker/model/linear_motion_tracker.cpp | 9 ++++++--- .../src/tracker/model/tracker_base.cpp | 3 +++ .../src/utils/radar_object_tracker_utils.cpp | 4 ++-- .../radar_object_tracker/src/utils/utils.cpp | 4 ++-- .../test/test_radar_object_tracker_utils.cpp | 2 +- 21 files changed, 67 insertions(+), 39 deletions(-) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 813f35bad0789..d5afbc06cce28 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -43,7 +43,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "RadarObjectTrackerNode" + PLUGIN "autoware::radar_object_tracker::RadarObjectTrackerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp index 3b6b2bcb9eb19..f19dd71164d44 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp @@ -35,7 +35,8 @@ #include #include #include - +namespace autoware::radar_object_tracker +{ class DataAssociation { private: @@ -63,5 +64,5 @@ class DataAssociation const std::string & file_name); virtual ~DataAssociation() {} }; - +} // namespace autoware::radar_object_tracker #endif // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp index 587ea6692da87..6244cfd29be1e 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp @@ -18,7 +18,7 @@ #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 // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp index 4567f1f50a439..bbbb3f9b14999 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp @@ -20,7 +20,7 @@ #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 // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp index 61a2bfa21dfef..b32a76f6d7774 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp @@ -20,7 +20,7 @@ #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 // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp index b69d88c14bd53..77487fffc7267 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp @@ -49,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; @@ -130,4 +132,6 @@ class RadarObjectTrackerNode : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; +} // namespace autoware::radar_object_tracker + #endif // AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 index 7bc262672e74d..6ae9cb021da1a 100644 --- a/perception/radar_object_tracker/include/autoware/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 @@ -19,7 +19,8 @@ #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 { @@ -107,5 +108,5 @@ class ConstantTurnRateMotionTracker : public Tracker // means constant turn rat autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~ConstantTurnRateMotionTracker() {} }; - +} // 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/autoware/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 index 35a1fed3409d1..2e7247c908f4b 100644 --- a/perception/radar_object_tracker/include/autoware/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 @@ -20,6 +20,9 @@ #include +namespace autoware::radar_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; class LinearMotionTracker : public Tracker { @@ -110,5 +113,5 @@ class LinearMotionTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~LinearMotionTracker() {} }; - +} // namespace autoware::radar_object_tracker #endif // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/tracker/model/tracker_base.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/tracker/model/tracker_base.hpp index 96aa9ac7c08d2..f106bc0a3d2fd 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/tracker/model/tracker_base.hpp @@ -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; }; - +} // namespace autoware::radar_object_tracker #endif // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 index 743db51188fed..b8d2f1f4d44ef 100644 --- a/perception/radar_object_tracker/include/autoware/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 @@ -46,7 +46,7 @@ #include #include -namespace radar_object_tracker_utils +namespace autoware::radar_object_tracker::utils { boost::optional getTransformAnonymous( @@ -71,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 // AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp index b4c2b9cdfc470..667cc34226681 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp @@ -33,8 +33,7 @@ #include #include #include - -namespace utils +namespace autoware::radar_object_tracker::utils { enum MSG_COV_IDX { X_X = 0, @@ -79,6 +78,6 @@ enum MSG_COV_IDX { Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices); Eigen::MatrixXd stackMatricesDiagonally(const std::vector & matrices); -} // namespace utils +} // namespace autoware::radar_object_tracker::utils #endif // AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/radar_object_tracker/src/association/data_association.cpp b/perception/radar_object_tracker/src/association/data_association.cpp index 78f2aaaa6b349..087e632235988 100644 --- a/perception/radar_object_tracker/src/association/data_association.cpp +++ b/perception/radar_object_tracker/src/association/data_association.cpp @@ -69,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, @@ -292,3 +294,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::radar_object_tracker diff --git a/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp index cb8a2967208c5..2a238b7b41513 100644 --- a/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp +++ b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp @@ -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/association/ssp/successive_shortest_path.cpp b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index a5cfa65a4ca2d..28f4ca7820694 100644 --- a/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp +++ b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp @@ -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.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp index cbe8183500fcc..9005411066c6c 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include @@ -38,6 +37,8 @@ #include #include +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options) @@ -141,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; @@ -237,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; @@ -284,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: " << @@ -422,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/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 176ca2ca34465..3703ba1fb5905 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 @@ -42,6 +42,8 @@ #include +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; // init static member variables @@ -442,9 +444,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)) { @@ -622,3 +624,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 a693c584a96a3..8819880943f99 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 @@ -43,6 +43,8 @@ #include +namespace autoware::radar_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; // initialize static parameter @@ -468,9 +470,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)) { @@ -688,3 +690,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 15742c5a191e7..1d47125abe930 100644 --- a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp @@ -21,6 +21,8 @@ #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 5fa3c91e3a3d9..7ad3e127ebd96 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 @@ -14,7 +14,7 @@ #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 199a32ee0c9fa..63aebdd2f03ae 100644 --- a/perception/radar_object_tracker/src/utils/utils.cpp +++ b/perception/radar_object_tracker/src/utils/utils.cpp @@ -14,7 +14,7 @@ #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 9f35e31641638..7505fd69859d6 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 @@ -16,8 +16,8 @@ #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) From 7f78781640fd8a6274ce302c9eebfbdc539e2953 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 17:54:52 +0900 Subject: [PATCH 4/6] refactor: remove MSG_COV_IDX enum in utils.hpp Signed-off-by: Taekjin LEE --- .../radar_object_tracker/utils/utils.hpp | 39 --------- .../constant_turn_rate_motion_tracker.cpp | 56 +++++++------ .../tracker/model/linear_motion_tracker.cpp | 83 ++++++++++--------- 3 files changed, 71 insertions(+), 107 deletions(-) diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp index 667cc34226681..b56f42e2e4031 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp @@ -35,45 +35,6 @@ #include 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); 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 3703ba1fb5905..70cbb7e71ed31 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 @@ -23,6 +23,7 @@ #include "autoware/radar_object_tracker/utils/utils.hpp" #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 #include @@ -45,6 +46,7 @@ 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; @@ -116,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 @@ -130,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; @@ -373,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); @@ -429,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); } @@ -584,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) { 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 8819880943f99..8141c84c01011 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 @@ -21,9 +21,9 @@ #include "autoware/radar_object_tracker/tracker/model/linear_motion_tracker.hpp" #include "autoware/radar_object_tracker/utils/utils.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 #include @@ -46,6 +46,7 @@ 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; @@ -122,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 @@ -133,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 { @@ -145,10 +146,10 @@ LinearMotionTracker::LinearMotionTracker( false; // currently message does not have acceleration covariance if (has_acceleration_covariance) { // const auto ax_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This + // object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; // This // is future update // const auto ay_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This + // object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; // This // is future update // Eigen::Matrix2d P_a_xy_local; // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; @@ -418,11 +419,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); @@ -450,8 +451,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); @@ -640,37 +641,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) { From c2f46f3293c30eb04667184ad587bcc808872319 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 25 Jun 2024 14:01:41 +0900 Subject: [PATCH 5/6] refactor: fix the node name Signed-off-by: Taekjin LEE --- perception/radar_object_tracker/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index d5afbc06cce28..b732ef6cc863c 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -44,7 +44,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::radar_object_tracker::RadarObjectTrackerNode" - EXECUTABLE ${PROJECT_NAME}_node + EXECUTABLE radar_object_tracker_node ) # testing From 1c4d662f5cdbe579ed78bb283998d9a08670c061 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 26 Jun 2024 14:02:04 +0900 Subject: [PATCH 6/6] refactor: update include paths for radar_object_tracker files Signed-off-by: Taekjin LEE --- .../association/data_association.hpp | 10 +++++----- .../association/solver/gnn_solver.hpp | 12 ++++++------ .../association/solver/gnn_solver_interface.hpp | 6 +++--- .../association/solver/mu_ssp.hpp | 8 ++++---- .../association/solver/ssp.hpp | 8 ++++---- .../model/constant_turn_rate_motion_tracker.hpp | 8 ++++---- .../tracker/model/linear_motion_tracker.hpp | 8 ++++---- .../tracker/model/tracker_base.hpp | 8 ++++---- .../tracker/tracker.hpp | 6 +++--- .../utils/radar_object_tracker_utils.hpp | 6 +++--- .../utils/utils.hpp | 6 +++--- perception/radar_object_tracker/package.xml | 1 - .../src/association/data_association.cpp | 4 ++-- .../mu_ssp/mu_successive_shortest_path_wrapper.cpp | 2 +- .../src/association/ssp/successive_shortest_path.cpp | 2 +- .../src/radar_object_tracker_node.cpp | 6 +++--- .../radar_object_tracker_node.hpp | 8 ++++---- .../model/constant_turn_rate_motion_tracker.cpp | 4 ++-- .../src/tracker/model/linear_motion_tracker.cpp | 4 ++-- .../src/tracker/model/tracker_base.cpp | 4 ++-- .../src/utils/radar_object_tracker_utils.cpp | 2 +- perception/radar_object_tracker/src/utils/utils.cpp | 2 +- .../test/test_radar_object_tracker_utils.cpp | 2 +- 23 files changed, 63 insertions(+), 64 deletions(-) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/association/data_association.hpp (85%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/association/solver/gnn_solver.hpp (59%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/association/solver/gnn_solver_interface.hpp (80%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/association/solver/mu_ssp.hpp (78%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/association/solver/ssp.hpp (79%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/tracker/model/constant_turn_rate_motion_tracker.hpp (90%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/tracker/model/linear_motion_tracker.hpp (91%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/tracker/model/tracker_base.hpp (91%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/tracker/tracker.hpp (79%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/utils/radar_object_tracker_utils.hpp (91%) rename perception/radar_object_tracker/include/{autoware/radar_object_tracker => autoware_radar_object_tracker}/utils/utils.hpp (88%) rename perception/radar_object_tracker/{include/autoware/radar_object_tracker => src}/radar_object_tracker_node.hpp (94%) diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp similarity index 85% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp index f19dd71164d44..efde1e6763cdd 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/data_association.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#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 "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 @@ -65,4 +65,4 @@ class DataAssociation virtual ~DataAssociation() {} }; } // namespace autoware::radar_object_tracker -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp similarity index 59% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp index d009b1e3ac60a..2aa4a577ef91c 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/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 AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__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 "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" +#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 // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 80% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp index 6244cfd29be1e..ee2e98ba47278 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__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 @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace autoware::radar_object_tracker::gnn_solver -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__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/autoware/radar_object_tracker/association/solver/mu_ssp.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp similarity index 78% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp index bbbb3f9b14999..2a5a336a3eb87 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/mu_ssp.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace autoware::radar_object_tracker::gnn_solver -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp similarity index 79% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp index b32a76f6d7774..c956e1ebd2f6d 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/association/solver/ssp.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "autoware/radar_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace autoware::radar_object_tracker::gnn_solver -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 90% rename from perception/radar_object_tracker/include/autoware/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 6ae9cb021da1a..dab4d3f8efa24 100644 --- a/perception/radar_object_tracker/include/autoware/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,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#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_ +#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 "autoware/radar_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" #include @@ -109,4 +109,4 @@ class ConstantTurnRateMotionTracker : public Tracker // means constant turn rat virtual ~ConstantTurnRateMotionTracker() {} }; } // namespace autoware::radar_object_tracker -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 91% rename from perception/radar_object_tracker/include/autoware/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 2e7247c908f4b..7c96aa8fbaa36 100644 --- a/perception/radar_object_tracker/include/autoware/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,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ -#define AUTOWARE__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 "autoware/radar_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" #include @@ -114,4 +114,4 @@ class LinearMotionTracker : public Tracker virtual ~LinearMotionTracker() {} }; } // namespace autoware::radar_object_tracker -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 91% rename from perception/radar_object_tracker/include/autoware/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 f106bc0a3d2fd..caea725ef8f81 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define AUTOWARE__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 "autoware_radar_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -95,4 +95,4 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; } // namespace autoware::radar_object_tracker -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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/autoware/radar_object_tracker/tracker/tracker.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp index 06dfec94bf83c..26222f4c65679 100644 --- a/perception/radar_object_tracker/include/autoware/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 AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define AUTOWARE__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 // AUTOWARE__RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/autoware/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 91% rename from perception/radar_object_tracker/include/autoware/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 b8d2f1f4d44ef..c3aaeb92ee951 100644 --- a/perception/radar_object_tracker/include/autoware/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,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ -#define AUTOWARE__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" @@ -73,4 +73,4 @@ bool hasValidVelocityDirectionToLanelet( } // namespace autoware::radar_object_tracker::utils -#endif // AUTOWARE__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/autoware/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp similarity index 88% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp rename to perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp index b56f42e2e4031..dd011ac274b64 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Eigen::MatrixXd stackMatricesDiagonally(const std::vector & mat } // namespace autoware::radar_object_tracker::utils -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ 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/association/data_association.cpp b/perception/radar_object_tracker/src/association/data_association.cpp index 087e632235988..ac57a3f0a87c0 100644 --- a/perception/radar_object_tracker/src/association/data_association.cpp +++ b/perception/radar_object_tracker/src/association/data_association.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/radar_object_tracker/association/data_association.hpp" +#include "autoware_radar_object_tracker/association/data_association.hpp" -#include "autoware/radar_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp" #include diff --git a/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp index 2a238b7b41513..2f47e3b8147d5 100644 --- a/perception/radar_object_tracker/src/association/mu_ssp/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 "autoware/radar_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware_radar_object_tracker/association/solver/mu_ssp.hpp" #include diff --git a/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp b/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index 28f4ca7820694..7877fe5451d8d 100644 --- a/perception/radar_object_tracker/src/association/ssp/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 "autoware/radar_object_tracker/association/solver/ssp.hpp" +#include "autoware_radar_object_tracker/association/solver/ssp.hpp" #include #include diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp index 9005411066c6c..10bf11e46abb9 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.cpp @@ -14,10 +14,10 @@ #define EIGEN_MPL2_ONLY -#include "autoware/radar_object_tracker/radar_object_tracker_node.hpp" +#include "radar_object_tracker_node.hpp" -#include "autoware/radar_object_tracker/utils/radar_object_tracker_utils.hpp" -#include "autoware/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 diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp similarity index 94% rename from perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp rename to perception/radar_object_tracker/src/radar_object_tracker_node.hpp index 77487fffc7267..dee2cd549c986 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ -#define AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ +#ifndef RADAR_OBJECT_TRACKER_NODE_HPP_ +#define RADAR_OBJECT_TRACKER_NODE_HPP_ -#include "autoware/radar_object_tracker/association/data_association.hpp" +#include "autoware_radar_object_tracker/association/data_association.hpp" #include #include @@ -134,4 +134,4 @@ class RadarObjectTrackerNode : public rclcpp::Node } // namespace autoware::radar_object_tracker -#endif // AUTOWARE__RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE_HPP_ +#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 70cbb7e71ed31..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 @@ -18,12 +18,12 @@ #define EIGEN_MPL2_ONLY -#include "autoware/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" +#include "autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" -#include "autoware/radar_object_tracker/utils/utils.hpp" #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 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 8141c84c01011..6866e70519d74 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 @@ -18,12 +18,12 @@ #define EIGEN_MPL2_ONLY -#include "autoware/radar_object_tracker/tracker/model/linear_motion_tracker.hpp" +#include "autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp" -#include "autoware/radar_object_tracker/utils/utils.hpp" #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 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 1d47125abe930..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,9 +14,9 @@ // // -#include "autoware/radar_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" #include #include 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 7ad3e127ebd96..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,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" 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 63aebdd2f03ae..351c4cfd21159 100644 --- a/perception/radar_object_tracker/src/utils/utils.cpp +++ b/perception/radar_object_tracker/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/radar_object_tracker/utils/utils.hpp" +#include "autoware_radar_object_tracker/utils/utils.hpp" 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 7505fd69859d6..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,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/radar_object_tracker/utils/radar_object_tracker_utils.hpp" +#include "autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp" #include