refactor(tracking_object_merger)!: fix namespace and directory structure

perception/tracking_object_merger/CMakeLists.txt

@@ -22,22 +22,22 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(decorative_tracker_merger_node SHARED
-  src/data_association/data_association.cpp
-  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
-  src/decorative_tracker_merger.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/association/data_association.cpp
+  src/association/solver/mu_successive_shortest_path_wrapper.cpp
+  src/decorative_tracker_merger_node.cpp
   src/utils/utils.cpp
   src/utils/tracker_state.cpp
 )
 
-target_link_libraries(decorative_tracker_merger_node
+target_link_libraries(${PROJECT_NAME}
   Eigen3::Eigen
   glog::glog
 )
 
-rclcpp_components_register_node(decorative_tracker_merger_node
-  PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode"
-  EXECUTABLE decorative_tracker_merger
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::tracking_object_merger::DecorativeTrackerMergerNode"
+  EXECUTABLE decorative_tracker_merger_node
 )
 
 ament_auto_package(INSTALL_TO_SHARE

perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp

@@ -16,26 +16,28 @@
 // Author: v1.0 Yukihiro Saito
 //
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
-
-#include <list>
-#include <memory>
-#include <unordered_map>
-#include <vector>
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_
 
 #define EIGEN_MPL2_ONLY
-#include "tracking_object_merger/data_association/solver/gnn_solver.hpp"
-#include "tracking_object_merger/utils/tracker_state.hpp"
+
+#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp"
+#include "autoware_tracking_object_merger/utils/tracker_state.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <autoware_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 
+#include <list>
+#include <memory>
 #include <string>
+#include <unordered_map>
+#include <vector>
 
+namespace autoware::tracking_object_merger
+{
 class DataAssociation
 {
 private:
@@ -68,4 +70,6 @@ class DataAssociation
   virtual ~DataAssociation() {}
 };
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
+}  // namespace autoware::tracking_object_merger
+
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_

perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp

@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
-#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp"
-#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp"
+#include "autoware_tracking_object_merger/association/solver/ssp.hpp"
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_

perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class GnnSolverInterface
 {
@@ -30,6 +30,6 @@ class GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) = 0;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class MuSSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_

perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
-#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_
 
-#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp"
 
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 class SSP : public GnnSolverInterface
 {
@@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface
     const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
     std::unordered_map<int, int> * reverse_assignment) override;
 };
-}  // namespace gnn_solver
+}  // namespace autoware::tracking_object_merger::gnn_solver
 
-#endif  // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_

perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp

@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
-#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/object_classification.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-#include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/object_classification.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <std_msgs/msg/header.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
@@ -32,6 +32,9 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+
+namespace autoware::tracking_object_merger
+{
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
 
@@ -144,5 +147,6 @@ class TrackerState
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
   std::vector<TrackerState> & tracker_states, const rclcpp::Time & time);
+}  // namespace autoware::tracking_object_merger
 
-#endif  // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_

perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp → perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp

@@ -14,19 +14,18 @@
 //
 //
 
-#ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
-#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
 
-// #include <autoware/universe_utils/autoware_universe_utils.hpp>
 #include "autoware/universe_utils/geometry/geometry.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_perception_msgs/msg/object_classification.hpp>
-#include <autoware_perception_msgs/msg/shape.hpp>
-#include <autoware_perception_msgs/msg/tracked_object.hpp>
-#include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include "autoware_perception_msgs/msg/object_classification.hpp"
+#include "autoware_perception_msgs/msg/shape.hpp"
+#include "autoware_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp"
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -42,49 +41,13 @@
 #include <tuple>
 #include <vector>
 
+namespace autoware::tracking_object_merger
+{
 using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
+
 namespace 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
-};
-
 // linear interpolation for tracked objects
 TrackedObject linearInterpolationForTrackedObject(
   const TrackedObject & obj1, const TrackedObject & obj2);
@@ -103,6 +66,9 @@ TrackedObjects interpolateTrackedObjects(
 
 namespace merger_utils
 {
+using autoware_perception_msgs::msg::TrackedObject;
+using autoware_perception_msgs::msg::TrackedObjects;
+
 // merge policy
 enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 };
 
@@ -132,4 +98,6 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su
 
 }  // namespace merger_utils
 
-#endif  // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_
+}  // namespace autoware::tracking_object_merger
+
+#endif  // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_

perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml

@@ -7,7 +7,7 @@
   <arg name="merge_options_path" default="$(find-pkg-share tracking_object_merger)/config/decorative_tracker_merger_policy.param.yaml"/>
   <arg name="node_param_file_path" default="$(find-pkg-share tracking_object_merger)/config/decorative_tracker_merger.param.yaml"/>
 
-  <node pkg="tracking_object_merger" exec="decorative_tracker_merger" name="$(anon decorative_tracker_merger)" output="screen">
+  <node pkg="tracking_object_merger" exec="decorative_tracker_merger_node" name="$(anon decorative_tracker_merger)" output="screen">
     <remap from="input/main_object" to="$(var input/main_object)"/>
     <remap from="input/sub_object" to="$(var input/sub_object)"/>
     <remap from="output/object" to="$(var output)"/>

perception/tracking_object_merger/package.xml

@@ -23,7 +23,6 @@
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

perception/tracking_object_merger/src/data_association/data_association.cpp → perception/tracking_object_merger/src/association/data_association.cpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tracking_object_merger/data_association/data_association.hpp"
+#include "autoware_tracking_object_merger/association/data_association.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp"
+#include "autoware_tracking_object_merger/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tracking_object_merger/data_association/solver/gnn_solver.hpp"
-#include "tracking_object_merger/utils/utils.hpp"
 
 #include <algorithm>
 #include <fstream>
@@ -47,6 +47,8 @@ double getFormedYawAngle(
 }
 }  // namespace
 
+namespace autoware::tracking_object_merger
+{
 DataAssociation::DataAssociation(
   std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
   std::vector<double> max_rad_vector, std::vector<double> min_iou_vector,
@@ -229,3 +231,5 @@ double DataAssociation::calcScoreBetweenObjects(
   }
   return score;
 }
+
+}  // namespace autoware::tracking_object_merger

perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp → perception/tracking_object_merger/src/association/solver/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 "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp"
+#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp"
 
 #include <mussp/mussp.h>
 
@@ -24,7 +24,7 @@
 #include <unordered_map>
 #include <vector>
 
-namespace gnn_solver
+namespace autoware::tracking_object_merger::gnn_solver
 {
 void MuSSP::maximizeLinearAssignment(
   const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * 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::tracking_object_merger::gnn_solver