Skip to content

Commit 3683131

Browse files
SakodaShintaropre-commit-ci[bot]
SakodaShintaro
and
pre-commit-ci[bot]
authored
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 <shintaro.sakoda@tier4.jp> * Added a line break Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Added includes Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed types Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed includes Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed meaningless code Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed a way of waiting results Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed error handlings Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed position Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added NOLINT Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed sensor points Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed sample pcd Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to execute multi test cases Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Added includes Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed StubPcdLoader Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Refactored Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed pcd range Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameters Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed TransformBroadcaster to StaticTransformBroadcaster Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent b9f7ae6 commit 3683131

12 files changed

+621
-8
lines changed

‎localization/ndt_scan_matcher/CMakeLists.txt

+16-2
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,24 @@ if(BUILD_TESTING)
4141
test/test_ndt_scan_matcher_launch.py
4242
TIMEOUT "30"
4343
)
44+
45+
find_package(ament_cmake_gtest REQUIRED)
46+
ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation
47+
test/test_cases/standard_sequence_for_initial_pose_estimation.cpp
48+
src/particle.cpp
49+
src/ndt_scan_matcher_core.cpp
50+
src/map_update_module.cpp
51+
)
52+
ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly
53+
test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp
54+
src/particle.cpp
55+
src/ndt_scan_matcher_core.cpp
56+
src/map_update_module.cpp
57+
)
4458
endif()
4559

4660
ament_auto_package(
4761
INSTALL_TO_SHARE
48-
launch
49-
config
62+
launch
63+
config
5064
)

‎localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ class NDTScanMatcher : public rclcpp::Node
7474
pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
7575

7676
public:
77-
NDTScanMatcher();
77+
explicit NDTScanMatcher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
7878

7979
private:
8080
void service_ndt_align(

‎localization/ndt_scan_matcher/src/map_update_module.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,9 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
7777
ndt_ptr_.reset(new NdtType);
7878

7979
ndt_ptr_->setParams(param);
80+
if (input_source != nullptr) {
81+
ndt_ptr_->setInputSource(input_source);
82+
}
8083

8184
const bool updated = update_ndt(position, *ndt_ptr_);
8285
if (!updated) {
@@ -88,9 +91,6 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
8891
ndt_ptr_mutex_->unlock();
8992
return;
9093
}
91-
if (input_source != nullptr) {
92-
ndt_ptr_->setInputSource(input_source);
93-
}
9494
ndt_ptr_mutex_->unlock();
9595
need_rebuild_ = false;
9696
} else {

‎localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -64,8 +64,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(
6464
throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value.");
6565
}
6666

67-
NDTScanMatcher::NDTScanMatcher()
68-
: Node("ndt_scan_matcher"),
67+
NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options)
68+
: Node("ndt_scan_matcher", options),
6969
tf2_broadcaster_(*this),
7070
tf2_buffer_(this->get_clock()),
7171
tf2_listener_(tf2_buffer_),

‎localization/ndt_scan_matcher/test/stub_initialpose_client.hpp

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// Copyright 2024 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef STUB_INITIALPOSE_CLIENT_HPP_
16+
#define STUB_INITIALPOSE_CLIENT_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include "tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp"
21+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
22+
23+
#include <memory>
24+
25+
class StubInitialposeClient : public rclcpp::Node
26+
{
27+
using AlignSrv = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
28+
29+
public:
30+
StubInitialposeClient() : Node("stub_initialpose_client")
31+
{
32+
align_service_client_ = this->create_client<AlignSrv>("/ndt_align_srv");
33+
}
34+
35+
geometry_msgs::msg::PoseWithCovarianceStamped send_initialpose(
36+
const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose)
37+
{
38+
// wait for the service to be available
39+
while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
40+
if (!rclcpp::ok()) {
41+
throw std::runtime_error("Interrupted while waiting for the service.");
42+
}
43+
RCLCPP_INFO(this->get_logger(), "Waiting for service...");
44+
}
45+
46+
// send the request
47+
std::shared_ptr<AlignSrv::Request> request = std::make_shared<AlignSrv::Request>();
48+
request->pose_with_covariance = initialpose;
49+
auto result = align_service_client_->async_send_request(request);
50+
if (
51+
rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
52+
rclcpp::FutureReturnCode::SUCCESS) {
53+
throw std::runtime_error("Service call failed.");
54+
}
55+
return result.get()->pose_with_covariance;
56+
}
57+
58+
private:
59+
rclcpp::Client<AlignSrv>::SharedPtr align_service_client_;
60+
};
61+
62+
#endif // STUB_INITIALPOSE_CLIENT_HPP_

‎localization/ndt_scan_matcher/test/stub_pcd_loader.hpp

+88
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright 2024 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef STUB_PCD_LOADER_HPP_
16+
#define STUB_PCD_LOADER_HPP_
17+
18+
#include "test_util.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
22+
#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp"
23+
24+
#include <pcl_conversions/pcl_conversions.h>
25+
26+
#include <algorithm>
27+
#include <limits>
28+
29+
class StubPcdLoader : public rclcpp::Node
30+
{
31+
using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap;
32+
33+
public:
34+
StubPcdLoader() : Node("stub_pcd_loader")
35+
{
36+
get_differential_pcd_maps_service_ = create_service<GetDifferentialPointCloudMap>(
37+
"pcd_loader_service", std::bind(
38+
&StubPcdLoader::on_service_get_differential_point_cloud_map, this,
39+
std::placeholders::_1, std::placeholders::_2));
40+
}
41+
42+
private:
43+
rclcpp::Service<GetDifferentialPointCloudMap>::SharedPtr get_differential_pcd_maps_service_;
44+
45+
// NOLINTNEXTLINE
46+
bool on_service_get_differential_point_cloud_map(
47+
GetDifferentialPointCloudMap::Request::SharedPtr req,
48+
GetDifferentialPointCloudMap::Response::SharedPtr res)
49+
{
50+
const float offset_x = 100.0f;
51+
const float offset_y = 100.0f;
52+
53+
// If the requested area is outside of the offset, return an empty response.
54+
if (
55+
req->area.center_x - req->area.radius > offset_x ||
56+
req->area.center_x + req->area.radius < offset_x ||
57+
req->area.center_y - req->area.radius > offset_y ||
58+
req->area.center_y + req->area.radius < offset_y) {
59+
res->header.frame_id = "map";
60+
return true;
61+
}
62+
63+
autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id;
64+
pcd_map_cell_with_id.cell_id = "0";
65+
pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_half_cubic_pcd();
66+
for (auto & point : cloud.points) {
67+
point.x += offset_x;
68+
point.y += offset_y;
69+
}
70+
pcd_map_cell_with_id.metadata.min_x = std::numeric_limits<float>::max();
71+
pcd_map_cell_with_id.metadata.min_y = std::numeric_limits<float>::max();
72+
pcd_map_cell_with_id.metadata.max_x = std::numeric_limits<float>::lowest();
73+
pcd_map_cell_with_id.metadata.max_y = std::numeric_limits<float>::lowest();
74+
for (const auto & point : cloud.points) {
75+
pcd_map_cell_with_id.metadata.min_x = std::min(pcd_map_cell_with_id.metadata.min_x, point.x);
76+
pcd_map_cell_with_id.metadata.min_y = std::min(pcd_map_cell_with_id.metadata.min_y, point.y);
77+
pcd_map_cell_with_id.metadata.max_x = std::max(pcd_map_cell_with_id.metadata.max_x, point.x);
78+
pcd_map_cell_with_id.metadata.max_y = std::max(pcd_map_cell_with_id.metadata.max_y, point.y);
79+
}
80+
RCLCPP_INFO_STREAM(get_logger(), "cloud size: " << cloud.size());
81+
pcl::toROSMsg(cloud, pcd_map_cell_with_id.pointcloud);
82+
res->new_pointcloud_with_ids.push_back(pcd_map_cell_with_id);
83+
res->header.frame_id = "map";
84+
return true;
85+
}
86+
};
87+
88+
#endif // STUB_PCD_LOADER_HPP_

‎localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
// Copyright 2024 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef STUB_SENSOR_PCD_PUBLISHER_HPP_
16+
#define STUB_SENSOR_PCD_PUBLISHER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <sensor_msgs/msg/point_cloud2.hpp>
21+
22+
class StubSensorPcdPublisher : public rclcpp::Node
23+
{
24+
public:
25+
StubSensorPcdPublisher() : Node("stub_sensor_pcd_publisher")
26+
{
27+
pcd_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/points_raw", 10);
28+
}
29+
30+
void publish_pcd(const sensor_msgs::msg::PointCloud2 & pcd) { pcd_publisher_->publish(pcd); }
31+
32+
private:
33+
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
34+
};
35+
36+
#endif // STUB_SENSOR_PCD_PUBLISHER_HPP_

‎localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
// Copyright 2024 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef STUB_TRIGGER_NODE_CLIENT_HPP_
16+
#define STUB_TRIGGER_NODE_CLIENT_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <std_srvs/srv/set_bool.hpp>
21+
22+
#include <memory>
23+
24+
class StubTriggerNodeClient : public rclcpp::Node
25+
{
26+
using SetBool = std_srvs::srv::SetBool;
27+
28+
public:
29+
StubTriggerNodeClient() : Node("stub_trigger_node_client")
30+
{
31+
align_service_client_ = this->create_client<SetBool>("/trigger_node_srv");
32+
}
33+
34+
bool send_trigger_node(const bool & trigger)
35+
{
36+
// wait for the service to be available
37+
while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
38+
if (!rclcpp::ok()) {
39+
throw std::runtime_error("Interrupted while waiting for the service.");
40+
}
41+
RCLCPP_INFO(this->get_logger(), "Waiting for service...");
42+
}
43+
44+
// send the request
45+
std::shared_ptr<SetBool::Request> request = std::make_shared<SetBool::Request>();
46+
request->data = trigger;
47+
auto result = align_service_client_->async_send_request(request);
48+
if (
49+
rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
50+
rclcpp::FutureReturnCode::SUCCESS) {
51+
throw std::runtime_error("Service call failed.");
52+
}
53+
return result.get()->success;
54+
}
55+
56+
private:
57+
rclcpp::Client<SetBool>::SharedPtr align_service_client_;
58+
};
59+
60+
#endif // STUB_TRIGGER_NODE_CLIENT_HPP_

0 commit comments

Comments
 (0)