test(ndt_scan_matcher): added an unit test to ndt_scan_matcher (#6649)

* Added an unit test to ndt_scan_matcher Signed-off-by: Shintaro Sakoda <[email protected]> * Added a line break Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added includes Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed types Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed includes Signed-off-by: Shintaro Sakoda <[email protected]> * Removed meaningless code Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed a way of waiting results Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed error handlings Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed position Signed-off-by: Shintaro Sakoda <[email protected]> * Added NOLINT Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed sensor points Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed sample pcd Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to execute multi test cases Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added includes Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed StubPcdLoader Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed pcd range Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed parameters Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed TransformBroadcaster to StaticTransformBroadcaster Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Mar 25, 2024 · 3683131 · 3683131
1 parent b9f7ae6
commit 3683131
Show file tree

Hide file tree

Showing 12 changed files with 621 additions and 8 deletions.
diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt
@@ -41,10 +41,24 @@ if(BUILD_TESTING)
     test/test_ndt_scan_matcher_launch.py
     TIMEOUT "30"
   )
+
+  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/particle.cpp
+    src/ndt_scan_matcher_core.cpp
+    src/map_update_module.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/particle.cpp
+    src/ndt_scan_matcher_core.cpp
+    src/map_update_module.cpp
+  )
 endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE
-    launch
-    config
+  launch
+  config
 )
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -74,7 +74,7 @@ class NDTScanMatcher : public rclcpp::Node
     pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
 
 public:
-  NDTScanMatcher();
+  explicit NDTScanMatcher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   void service_ndt_align(

diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -77,6 +77,9 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
     ndt_ptr_.reset(new NdtType);
 
     ndt_ptr_->setParams(param);
+    if (input_source != nullptr) {
+      ndt_ptr_->setInputSource(input_source);
+    }
 
     const bool updated = update_ndt(position, *ndt_ptr_);
     if (!updated) {
@@ -88,9 +91,6 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
       ndt_ptr_mutex_->unlock();
       return;
     }
-    if (input_source != nullptr) {
-      ndt_ptr_->setInputSource(input_source);
-    }
     ndt_ptr_mutex_->unlock();
     need_rebuild_ = false;
   } else {

diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -64,8 +64,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(
   throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value.");
 }
 
-NDTScanMatcher::NDTScanMatcher()
-: Node("ndt_scan_matcher"),
+NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options)
+: Node("ndt_scan_matcher", options),
   tf2_broadcaster_(*this),
   tf2_buffer_(this->get_clock()),
   tf2_listener_(tf2_buffer_),

diff --git a/localization/ndt_scan_matcher/test/stub_initialpose_client.hpp b/localization/ndt_scan_matcher/test/stub_initialpose_client.hpp
@@ -0,0 +1,62 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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.
+
+#ifndef STUB_INITIALPOSE_CLIENT_HPP_
+#define STUB_INITIALPOSE_CLIENT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <memory>
+
+class StubInitialposeClient : public rclcpp::Node
+{
+  using AlignSrv = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+public:
+  StubInitialposeClient() : Node("stub_initialpose_client")
+  {
+    align_service_client_ = this->create_client<AlignSrv>("/ndt_align_srv");
+  }
+
+  geometry_msgs::msg::PoseWithCovarianceStamped send_initialpose(
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose)
+  {
+    // wait for the service to be available
+    while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
+      if (!rclcpp::ok()) {
+        throw std::runtime_error("Interrupted while waiting for the service.");
+      }
+      RCLCPP_INFO(this->get_logger(), "Waiting for service...");
+    }
+
+    // send the request
+    std::shared_ptr<AlignSrv::Request> request = std::make_shared<AlignSrv::Request>();
+    request->pose_with_covariance = initialpose;
+    auto result = align_service_client_->async_send_request(request);
+    if (
+      rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+      rclcpp::FutureReturnCode::SUCCESS) {
+      throw std::runtime_error("Service call failed.");
+    }
+    return result.get()->pose_with_covariance;
+  }
+
+private:
+  rclcpp::Client<AlignSrv>::SharedPtr align_service_client_;
+};
+
+#endif  // STUB_INITIALPOSE_CLIENT_HPP_
diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp
@@ -0,0 +1,88 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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.
+
+#ifndef STUB_PCD_LOADER_HPP_
+#define STUB_PCD_LOADER_HPP_
+
+#include "test_util.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <algorithm>
+#include <limits>
+
+class StubPcdLoader : public rclcpp::Node
+{
+  using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap;
+
+public:
+  StubPcdLoader() : Node("stub_pcd_loader")
+  {
+    get_differential_pcd_maps_service_ = create_service<GetDifferentialPointCloudMap>(
+      "pcd_loader_service", std::bind(
+                              &StubPcdLoader::on_service_get_differential_point_cloud_map, this,
+                              std::placeholders::_1, std::placeholders::_2));
+  }
+
+private:
+  rclcpp::Service<GetDifferentialPointCloudMap>::SharedPtr get_differential_pcd_maps_service_;
+
+  // NOLINTNEXTLINE
+  bool on_service_get_differential_point_cloud_map(
+    GetDifferentialPointCloudMap::Request::SharedPtr req,
+    GetDifferentialPointCloudMap::Response::SharedPtr res)
+  {
+    const float offset_x = 100.0f;
+    const float offset_y = 100.0f;
+
+    // If the requested area is outside of the offset, return an empty response.
+    if (
+      req->area.center_x - req->area.radius > offset_x ||
+      req->area.center_x + req->area.radius < offset_x ||
+      req->area.center_y - req->area.radius > offset_y ||
+      req->area.center_y + req->area.radius < offset_y) {
+      res->header.frame_id = "map";
+      return true;
+    }
+
+    autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id;
+    pcd_map_cell_with_id.cell_id = "0";
+    pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_half_cubic_pcd();
+    for (auto & point : cloud.points) {
+      point.x += offset_x;
+      point.y += offset_y;
+    }
+    pcd_map_cell_with_id.metadata.min_x = std::numeric_limits<float>::max();
+    pcd_map_cell_with_id.metadata.min_y = std::numeric_limits<float>::max();
+    pcd_map_cell_with_id.metadata.max_x = std::numeric_limits<float>::lowest();
+    pcd_map_cell_with_id.metadata.max_y = std::numeric_limits<float>::lowest();
+    for (const auto & point : cloud.points) {
+      pcd_map_cell_with_id.metadata.min_x = std::min(pcd_map_cell_with_id.metadata.min_x, point.x);
+      pcd_map_cell_with_id.metadata.min_y = std::min(pcd_map_cell_with_id.metadata.min_y, point.y);
+      pcd_map_cell_with_id.metadata.max_x = std::max(pcd_map_cell_with_id.metadata.max_x, point.x);
+      pcd_map_cell_with_id.metadata.max_y = std::max(pcd_map_cell_with_id.metadata.max_y, point.y);
+    }
+    RCLCPP_INFO_STREAM(get_logger(), "cloud size: " << cloud.size());
+    pcl::toROSMsg(cloud, pcd_map_cell_with_id.pointcloud);
+    res->new_pointcloud_with_ids.push_back(pcd_map_cell_with_id);
+    res->header.frame_id = "map";
+    return true;
+  }
+};
+
+#endif  // STUB_PCD_LOADER_HPP_
diff --git a/localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp b/localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp
@@ -0,0 +1,36 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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.
+
+#ifndef STUB_SENSOR_PCD_PUBLISHER_HPP_
+#define STUB_SENSOR_PCD_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+class StubSensorPcdPublisher : public rclcpp::Node
+{
+public:
+  StubSensorPcdPublisher() : Node("stub_sensor_pcd_publisher")
+  {
+    pcd_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/points_raw", 10);
+  }
+
+  void publish_pcd(const sensor_msgs::msg::PointCloud2 & pcd) { pcd_publisher_->publish(pcd); }
+
+private:
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
+};
+
+#endif  // STUB_SENSOR_PCD_PUBLISHER_HPP_
diff --git a/localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp b/localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp
@@ -0,0 +1,60 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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.
+
+#ifndef STUB_TRIGGER_NODE_CLIENT_HPP_
+#define STUB_TRIGGER_NODE_CLIENT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_srvs/srv/set_bool.hpp>
+
+#include <memory>
+
+class StubTriggerNodeClient : public rclcpp::Node
+{
+  using SetBool = std_srvs::srv::SetBool;
+
+public:
+  StubTriggerNodeClient() : Node("stub_trigger_node_client")
+  {
+    align_service_client_ = this->create_client<SetBool>("/trigger_node_srv");
+  }
+
+  bool send_trigger_node(const bool & trigger)
+  {
+    // wait for the service to be available
+    while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
+      if (!rclcpp::ok()) {
+        throw std::runtime_error("Interrupted while waiting for the service.");
+      }
+      RCLCPP_INFO(this->get_logger(), "Waiting for service...");
+    }
+
+    // send the request
+    std::shared_ptr<SetBool::Request> request = std::make_shared<SetBool::Request>();
+    request->data = trigger;
+    auto result = align_service_client_->async_send_request(request);
+    if (
+      rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+      rclcpp::FutureReturnCode::SUCCESS) {
+      throw std::runtime_error("Service call failed.");
+    }
+    return result.get()->success;
+  }
+
+private:
+  rclcpp::Client<SetBool>::SharedPtr align_service_client_;
+};
+
+#endif  // STUB_TRIGGER_NODE_CLIENT_HPP_