diff --git a/perception/object_merger/CMakeLists.txt b/perception/object_merger/CMakeLists.txt index b02983db11af3..2d9b490945caf 100644 --- a/perception/object_merger/CMakeLists.txt +++ b/perception/object_merger/CMakeLists.txt @@ -18,18 +18,18 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(object_association_merger SHARED - src/object_association_merger/data_association/data_association.cpp - src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/object_association_merger/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/association/data_association.cpp + src/association/solver/mu_successive_shortest_path_wrapper.cpp + src/object_association_merger_node.cpp ) -target_link_libraries(object_association_merger +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen ) -rclcpp_components_register_node(object_association_merger - PLUGIN "object_association::ObjectAssociationMergerNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::object_merger::ObjectAssociationMergerNode" EXECUTABLE object_association_merger_node ) diff --git a/perception/object_merger/include/object_merger/data_association/data_association.hpp b/perception/object_merger/include/autoware_object_merger/association/data_association.hpp similarity index 75% rename from perception/object_merger/include/object_merger/data_association/data_association.hpp rename to perception/object_merger/include/autoware_object_merger/association/data_association.hpp index f8eec33f4a71e..8433e8f7af537 100644 --- a/perception/object_merger/include/object_merger/data_association/data_association.hpp +++ b/perception/object_merger/include/autoware_object_merger/association/data_association.hpp @@ -16,21 +16,25 @@ // Author: v1.0 Yukihiro Saito // -#ifndef OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ - -#include -#include -#include -#include +#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "object_merger/data_association/solver/gnn_solver.hpp" + +#include "autoware_object_merger/association/solver/gnn_solver.hpp" #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include + +namespace autoware::object_merger +{ class DataAssociation { @@ -40,7 +44,7 @@ class DataAssociation Eigen::MatrixXd max_rad_matrix_; Eigen::MatrixXd min_iou_matrix_; const double score_threshold_; - std::unique_ptr gnn_solver_ptr_; + std::unique_ptr gnn_solver_ptr_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -56,4 +60,6 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::object_merger + +#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp similarity index 57% rename from perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp rename to perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp index cead11d516421..6421dd6108a4a 100644 --- a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp +++ b/perception/object_merger/include/autoware_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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "object_merger/data_association/solver/gnn_solver_interface.hpp" -#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp" -#include "object_merger/data_association/solver/successive_shortest_path.hpp" +#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware_object_merger/association/solver/mu_ssp.hpp" +#include "autoware_object_merger/association/solver/ssp.hpp" -#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp similarity index 73% rename from perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp rename to perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp index 447f4fa2b7ce3..6a0702f056ba7 100644 --- a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp +++ b/perception/object_merger/include/autoware_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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include -namespace gnn_solver +namespace autoware::object_merger::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::object_merger::gnn_solver -#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp similarity index 71% rename from perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp rename to perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp index 0464a29e6b2e5..364e9306112a7 100644 --- a/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/object_merger/include/autoware_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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" #include #include -namespace gnn_solver +namespace autoware::object_merger::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::object_merger::gnn_solver -#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp b/perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp similarity index 71% rename from perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp rename to perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp index 618879a01866f..41bf390829ade 100644 --- a/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp +++ b/perception/object_merger/include/autoware_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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" #include #include -namespace gnn_solver +namespace autoware::object_merger::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::object_merger::gnn_solver -#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/object_merger/include/object_merger/utils/utils.hpp b/perception/object_merger/include/object_merger/utils/utils.hpp deleted file mode 100644 index bb167cbe20bd6..0000000000000 --- a/perception/object_merger/include/object_merger/utils/utils.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// -// Author: v1.0 Yukihiro Saito -// - -#ifndef OBJECT_MERGER__UTILS__UTILS_HPP_ -#define OBJECT_MERGER__UTILS__UTILS_HPP_ - -#include -#include -#include -#include - -#include -#include -#include - -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 -}; -} // namespace utils - -#endif // OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index 0340a477936b8..5ca261b3ddf0e 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -22,7 +22,6 @@ rclcpp_components tf2 tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/association/data_association.cpp similarity index 95% rename from perception/object_merger/src/object_association_merger/data_association/data_association.cpp rename to perception/object_merger/src/association/data_association.cpp index c32fe480ff4af..c2f7d44fa779b 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/association/data_association.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_merger/data_association/data_association.hpp" +#include "autoware_object_merger/association/data_association.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_merger/data_association/solver/gnn_solver.hpp" -#include "object_merger/utils/utils.hpp" +#include "autoware_object_merger/association/solver/gnn_solver.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -47,6 +46,9 @@ double getFormedYawAngle( } } // namespace +namespace autoware::object_merger +{ + DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_rad_vector, std::vector min_iou_vector) @@ -77,7 +79,7 @@ DataAssociation::DataAssociation( min_iou_matrix_ = min_iou_matrix_tmp.transpose(); } - gnn_solver_ptr_ = std::make_unique(); + gnn_solver_ptr_ = std::make_unique(); } void DataAssociation::assign( @@ -172,3 +174,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::object_merger diff --git a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp similarity index 87% rename from perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp index e7fa56fee88c5..6a5dd6473da1f 100644 --- a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/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 "object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware_object_merger/association/solver/mu_ssp.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace gnn_solver +namespace autoware::object_merger::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::object_merger::gnn_solver diff --git a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_merger/src/association/solver/successive_shortest_path.cpp similarity index 98% rename from perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/object_merger/src/association/solver/successive_shortest_path.cpp index b38257d0188e0..782aa1ef4ac31 100644 --- a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/object_merger/src/association/solver/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_merger/data_association/solver/successive_shortest_path.hpp" +#include "autoware_object_merger/association/solver/ssp.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace gnn_solver +namespace autoware::object_merger::gnn_solver { struct ResidualEdge { @@ -367,4 +367,4 @@ void SSP::maximizeLinearAssignment( } #endif } -} // namespace gnn_solver +} // namespace autoware::object_merger::gnn_solver diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger_node.cpp similarity index 97% rename from perception/object_merger/src/object_association_merger/node.cpp rename to perception/object_merger/src/object_association_merger_node.cpp index 2b806382382cf..321f1a8da8d9e 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger_node.cpp @@ -12,20 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_merger/node.hpp" +#define EIGEN_MPL2_ONLY + +#include "object_association_merger_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#include +#include + #include #include #include - -#define EIGEN_MPL2_ONLY -#include -#include +#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -70,7 +71,7 @@ std::map convertListToClassMap(const std::vector & distance } } // namespace -namespace object_association +namespace autoware::object_merger { ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptions & node_options) : rclcpp::Node("object_association_merger_node", node_options), @@ -104,7 +105,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - data_association_ = std::make_unique( + data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); // Create publishers and subscribers @@ -231,7 +232,7 @@ void ObjectAssociationMergerNode::objectsCallback( processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } -} // namespace object_association +} // namespace autoware::object_merger #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_association::ObjectAssociationMergerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::object_merger::ObjectAssociationMergerNode) diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/src/object_association_merger_node.hpp similarity index 85% rename from perception/object_merger/include/object_merger/node.hpp rename to perception/object_merger/src/object_association_merger_node.hpp index 2f5918a9c7f17..d5b5a1ae018a6 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/src/object_association_merger_node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_MERGER__NODE_HPP_ -#define OBJECT_MERGER__NODE_HPP_ +#ifndef OBJECT_ASSOCIATION_MERGER_NODE_HPP_ +#define OBJECT_ASSOCIATION_MERGER_NODE_HPP_ -#include "object_merger/data_association/data_association.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_object_merger/association/data_association.hpp" -#include -#include -#include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -45,7 +45,7 @@ #include #include -namespace object_association +namespace autoware::object_merger { class ObjectAssociationMergerNode : public rclcpp::Node { @@ -89,6 +89,6 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_association +} // namespace autoware::object_merger -#endif // OBJECT_MERGER__NODE_HPP_ +#endif // OBJECT_ASSOCIATION_MERGER_NODE_HPP_