Skip to content

Commit

Permalink
refactor(ndt_scan_marcher): add namespace "autoware::ndt_scan_matcher…
Browse files Browse the repository at this point in the history
…::" (autowarefoundation#8691)

* Added namespace "autoware::ndt_scan_matcher::"

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added namespace "autoware::ndt_scan_matcher::"

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
  • Loading branch information
SakodaShintaro authored Sep 3, 2024
1 parent 58297a0 commit 757a5a5
Show file tree
Hide file tree
Showing 9 changed files with 38 additions and 4 deletions.
2 changes: 1 addition & 1 deletion localization/ndt_scan_matcher/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ link_directories(${PCL_LIBRARY_DIRS})
target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES})

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "NDTScanMatcher"
PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher"
EXECUTABLE ${PROJECT_NAME}_node
EXECUTOR MultiThreadedExecutor
)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@
#include <string>
#include <vector>

namespace autoware::ndt_scan_matcher
{

enum class ConvergedParamType {
TRANSFORM_PROBABILITY = 0,
NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
Expand Down Expand Up @@ -191,4 +194,6 @@ struct HyperParameters
}
};

} // namespace autoware::ndt_scan_matcher

#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@
#include <thread>
#include <vector>

namespace autoware::ndt_scan_matcher
{

class MapUpdateModule
{
using PointSource = pcl::PointXYZ;
Expand Down Expand Up @@ -95,4 +98,6 @@ class MapUpdateModule
std::mutex last_update_position_mtx_;
};

} // namespace autoware::ndt_scan_matcher

#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,9 @@
#include <tuple>
#include <vector>

namespace autoware::ndt_scan_matcher
{

class NDTScanMatcher : public rclcpp::Node
{
using PointSource = pcl::PointXYZ;
Expand Down Expand Up @@ -209,4 +212,6 @@ class NDTScanMatcher : public rclcpp::Node
HyperParameters param_;
};

} // namespace autoware::ndt_scan_matcher

#endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,9 @@

#include <string>

namespace autoware::ndt_scan_matcher
{

struct Particle
{
Particle(
Expand All @@ -38,4 +41,6 @@ void push_debug_markers(
visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp,
const std::string & map_frame_, const Particle & particle, const size_t i);

} // namespace autoware::ndt_scan_matcher

#endif // NDT_SCAN_MATCHER__PARTICLE_HPP_
5 changes: 5 additions & 0 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@

#include "ndt_scan_matcher/map_update_module.hpp"

namespace autoware::ndt_scan_matcher
{

MapUpdateModule::MapUpdateModule(
rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr,
HyperParameters::DynamicMapLoading param)
Expand Down Expand Up @@ -312,3 +315,5 @@ void MapUpdateModule::publish_partial_pcd_map()

loaded_pcd_pub_->publish(map_msg);
}

} // namespace autoware::ndt_scan_matcher
7 changes: 6 additions & 1 deletion localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@
#include <iomanip>
#include <thread>

namespace autoware::ndt_scan_matcher
{

tier4_debug_msgs::msg::Float32Stamped make_float32_stamped(
const builtin_interfaces::msg::Time & stamp, const float data)
{
Expand Down Expand Up @@ -1111,5 +1114,7 @@ std::tuple<geometry_msgs::msg::PoseWithCovarianceStamped, double> NDTScanMatcher
return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score);
}

} // namespace autoware::ndt_scan_matcher

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher)
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ndt_scan_matcher::NDTScanMatcher)
4 changes: 4 additions & 0 deletions localization/ndt_scan_matcher/src/particle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#include "ndt_scan_matcher/particle.hpp"

#include "localization_util/util_func.hpp"
namespace autoware::ndt_scan_matcher
{

void push_debug_markers(
visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp,
Expand Down Expand Up @@ -61,3 +63,5 @@ void push_debug_markers(
marker.color = exchange_color_crc(static_cast<double>(i) / 100.0);
marker_array.markers.push_back(marker);
}

} // namespace autoware::ndt_scan_matcher
4 changes: 2 additions & 2 deletions localization/ndt_scan_matcher/test/test_fixture.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ class TestNDTScanMatcher : public ::testing::Test
node_options.parameter_overrides().push_back(param);
}
}
node_ = std::make_shared<NDTScanMatcher>(node_options);
node_ = std::make_shared<autoware::ndt_scan_matcher::NDTScanMatcher>(node_options);
rcl_yaml_node_struct_fini(params_st);

// prepare tf_static "base_link -> sensor_frame"
Expand All @@ -79,7 +79,7 @@ class TestNDTScanMatcher : public ::testing::Test
sensor_pcd_publisher_ = std::make_shared<StubSensorPcdPublisher>();
}

std::shared_ptr<NDTScanMatcher> node_;
std::shared_ptr<autoware::ndt_scan_matcher::NDTScanMatcher> node_;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
std::shared_ptr<StubPcdLoader> pcd_loader_;
std::shared_ptr<StubInitialposeClient> initialpose_client_;
Expand Down

0 comments on commit 757a5a5

Please sign in to comment.