Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(ndt_scan_matcher): componentize NDTScanMatcher #7130

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 8 additions & 14 deletions localization/ndt_scan_matcher/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,20 +22,24 @@ else()
endif()
endif()

find_package(glog REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io registration)
include_directories(${PCL_INCLUDE_DIRS})

ament_auto_add_executable(ndt_scan_matcher
ament_auto_add_library(${PROJECT_NAME} SHARED
src/diagnostics_module.cpp
src/map_update_module.cpp
src/ndt_scan_matcher_core.cpp
src/ndt_scan_matcher_node.cpp
src/particle.cpp
)

link_directories(${PCL_LIBRARY_DIRS})
target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog)
target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES})

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "NDTScanMatcher"
EXECUTABLE ${PROJECT_NAME}_node
EXECUTOR MultiThreadedExecutor
)

if(BUILD_TESTING)
add_launch_test(
Expand All @@ -46,19 +50,9 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation
test/test_cases/standard_sequence_for_initial_pose_estimation.cpp
src/diagnostics_module.cpp
src/map_update_module.cpp
src/ndt_scan_matcher_core.cpp
# src/ndt_scan_matcher_node.cpp
src/particle.cpp
)
ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly
test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp
src/diagnostics_module.cpp
src/map_update_module.cpp
src/ndt_scan_matcher_core.cpp
# src/ndt_scan_matcher_node.cpp
src/particle.cpp
)
endif()

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

<arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>

<node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
<node pkg="ndt_scan_matcher" exec="ndt_scan_matcher_node" name="$(var node_name)" output="both">
<remap from="points_raw" to="$(var input_pointcloud)"/>
<remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
<remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
Expand Down
1 change: 1 addition & 0 deletions localization/ndt_scan_matcher/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
<depend>ndt_omp</depend>
<depend>pcl_conversions</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
Expand Down
3 changes: 3 additions & 0 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1069,3 +1069,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(

return result_pose_with_cov_msg;
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher)
35 changes: 0 additions & 35 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp

This file was deleted.

Loading