Skip to content

Commit

Permalink
backup
Browse files Browse the repository at this point in the history
Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
  • Loading branch information
yukkysaito committed May 31, 2022
1 parent 2f39a35 commit e29cfaa
Show file tree
Hide file tree
Showing 14 changed files with 183 additions and 140 deletions.
28 changes: 25 additions & 3 deletions perception/object_merger/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,37 @@ project(object_merger)
find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(PCL REQUIRED COMPONENTS common filters)
# Ignore -Wnonportable-include-path in Clang for mussp
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wno-nonportable-include-path)
endif()

### Find Eigen Dependencies
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(
SYSTEM
${EIGEN3_INCLUDE_DIR}
)

ament_auto_add_library(object_association_merger SHARED
src/object_association_merger/utils/utils.cpp
src/object_association_merger/data_association/data_association.cpp
src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp
src/object_association_merger/node.cpp
)

target_link_libraries(object_association_merger ${PCL_LIBRARIES})
target_link_libraries(object_association_merger
mu_successive_shortest_path
Eigen3::Eigen
)

# workaround to allow deprecated header to build on both galactic and rolling
if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0)
target_compile_definitions(object_association_merger PUBLIC
USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
)
endif()

rclcpp_components_register_node(object_association_merger
PLUGIN "object_association::ObjectAssociationMergerNode"
Expand All @@ -22,4 +43,5 @@ rclcpp_components_register_node(object_association_merger

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
10 changes: 8 additions & 2 deletions perception/object_merger/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,14 @@ The successive shortest path algorithm is used to solve the data association pro

## Parameters

No Parameters.

| Name | Type | Description |
| --------------------------- | ------ | -------------------------------------------------------------- |
| `can_assign_matrix` | double | Assignment table for data association |
| `max_dist_matrix` | double | Maximum distance table for data association |
| `max_area_matrix` | double | Maximum area table for data association |
| `min_area_matrix` | double | Minimum area table for data association |
| `max_rad_matrix` | double | Maximum angle table for data association |
| `base_link_frame_id` | double | association frame |
## Assumptions / Known limits

<!-- Write assumptions and limitations of your implementation.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,16 @@
// Author: v1.0 Yukihiro Saito
//

#ifndef OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#define OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_

#include <list>
#include <memory>
#include <unordered_map>
#include <vector>

#define EIGEN_MPL2_ONLY
#include "object_merger/data_association/solver/gnn_solver.hpp"
#include "object_association_merger/data_association/solver/gnn_solver.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
Expand Down Expand Up @@ -54,9 +54,9 @@ class DataAssociation
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Eigen::MatrixXd calcScoreMatrix(
const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
const std::list<std::shared_ptr<Tracker>> & trackers);
const autoware_auto_perception_msgs::msg::DetectedObjects & objects0,
const autoware_auto_perception_msgs::msg::DetectedObjects & objects1);
virtual ~DataAssociation() {}
};

#endif // OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_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 "object_association_merger/data_association/solver/gnn_solver_interface.hpp"
#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp"
#include "object_association_merger/data_association/solver/successive_shortest_path.hpp"

#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,10 @@
// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_

#include "object_merger/data_association/solver/gnn_solver_interface.hpp"
#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp"

#include <unordered_map>
#include <vector>
Expand All @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface
};
} // namespace gnn_solver

#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,10 @@
// 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_

#include "object_merger/data_association/solver/gnn_solver_interface.hpp"
#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp"

#include <unordered_map>
#include <vector>
Expand All @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface
};
} // namespace gnn_solver

#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,31 @@
#ifndef OBJECT_ASSOCIATION_MERGER__NODE_HPP_
#define OBJECT_ASSOCIATION_MERGER__NODE_HPP_

#include <object_association_merger/data_association.hpp>
#include "object_association_merger/data_association/data_association.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2/transform_datatypes.h>

#ifdef USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <string>

namespace object_association
{
Expand All @@ -54,6 +66,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node
typedef message_filters::Synchronizer<SyncPolicy> Sync;
Sync sync_;
std::unique_ptr<DataAssociation> data_association_;
std::string base_link_frame_id_; // associated with the base_link frame
};
} // namespace object_association

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
// Author: v1.0 Yukihiro Saito
//

#ifndef OBJECT_MERGER__UTILS__UTILS_HPP_
#define OBJECT_MERGER__UTILS__UTILS_HPP_
#ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
#define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_

#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

Expand Down Expand Up @@ -89,4 +89,4 @@ enum MSG_COV_IDX {
};
} // namespace utils

#endif // OBJECT_MERGER__UTILS__UTILS_HPP_
#endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
10 changes: 5 additions & 5 deletions perception/object_merger/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,19 +8,19 @@
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>libpcl-all-dev</depend>
<depend>message_filters</depend>
<depend>pcl_conversions</depend>
<depend>eigen</depend>
<depend>mussp</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_perception_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
Loading

0 comments on commit e29cfaa

Please sign in to comment.