Skip to content

Commit b692872

Browse files
SakodaShintaroSakodaShintaro
committed
Added an unit test to ndt_scan_matcher
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent c24c3b0 commit b692872

9 files changed

+444
-3
lines changed

‎localization/ndt_scan_matcher/CMakeLists.txt

+12
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,18 @@ 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(test_ndt_scan_matcher
47+
test/test.cpp
48+
src/particle.cpp
49+
src/ndt_scan_matcher_core.cpp
50+
src/map_update_module.cpp
51+
)
52+
target_include_directories(test_ndt_scan_matcher
53+
SYSTEM PUBLIC
54+
)
55+
target_link_libraries(test_ndt_scan_matcher)
4456
endif()
4557

4658
ament_auto_package(

‎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/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

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
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 NDT_SCAN_MATCHER__TEST__STUB_INITIALPOSE_CLIENT_HPP_
16+
#define NDT_SCAN_MATCHER__TEST__STUB_INITIALPOSE_CLIENT_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
21+
22+
class StubInitialposeClient : public rclcpp::Node
23+
{
24+
using AlignSrv = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
25+
26+
public:
27+
StubInitialposeClient() : Node("stub_initialpose_client")
28+
{
29+
align_service_client_ = this->create_client<AlignSrv>("/ndt_align_srv");
30+
}
31+
32+
geometry_msgs::msg::PoseWithCovarianceStamped send_initialpose(
33+
const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose)
34+
{
35+
// wait for the service to be available
36+
while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
37+
if (!rclcpp::ok()) {
38+
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service.");
39+
EXIT_FAILURE;
40+
}
41+
RCLCPP_INFO(this->get_logger(), "Waiting for service...");
42+
}
43+
44+
// send the request
45+
std::shared_ptr<AlignSrv::Request> request = std::make_shared<AlignSrv::Request>();
46+
request->pose_with_covariance = initialpose;
47+
auto result = align_service_client_->async_send_request(request);
48+
49+
// wait for the response
50+
std::future_status status = result.wait_for(std::chrono::seconds(0));
51+
while (status != std::future_status::ready) {
52+
if (!rclcpp::ok()) {
53+
EXIT_FAILURE;
54+
}
55+
status = result.wait_for(std::chrono::seconds(1));
56+
}
57+
58+
return result.get()->pose_with_covariance;
59+
}
60+
61+
private:
62+
rclcpp::Client<AlignSrv>::SharedPtr align_service_client_;
63+
};
64+
65+
#endif // NDT_SCAN_MATCHER__TEST__STUB_INITIALPOSE_CLIENT_HPP_

‎localization/ndt_scan_matcher/test/stub_pcd_loader.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 NDT_SCAN_MATCHER__TEST__STUB_PCD_LOADER_HPP_
16+
#define NDT_SCAN_MATCHER__TEST__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+
class StubPcdLoader : public rclcpp::Node
25+
{
26+
using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap;
27+
28+
public:
29+
StubPcdLoader() : Node("stub_pcd_loader")
30+
{
31+
get_differential_pcd_maps_service_ = create_service<GetDifferentialPointCloudMap>(
32+
"pcd_loader_service", std::bind(
33+
&StubPcdLoader::on_service_get_differential_point_cloud_map, this,
34+
std::placeholders::_1, std::placeholders::_2));
35+
}
36+
37+
private:
38+
rclcpp::Service<GetDifferentialPointCloudMap>::SharedPtr get_differential_pcd_maps_service_;
39+
40+
bool on_service_get_differential_point_cloud_map(
41+
GetDifferentialPointCloudMap::Request::SharedPtr req,
42+
GetDifferentialPointCloudMap::Response::SharedPtr res)
43+
{
44+
(void)req;
45+
autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id;
46+
constexpr float max_position = 20.0f;
47+
constexpr float interval = 0.2f;
48+
pcd_map_cell_with_id.metadata.min_x = -max_position;
49+
pcd_map_cell_with_id.metadata.min_y = -max_position;
50+
pcd_map_cell_with_id.metadata.max_x = max_position;
51+
pcd_map_cell_with_id.metadata.max_y = max_position;
52+
pcd_map_cell_with_id.cell_id = "0";
53+
pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_pcd(-max_position, max_position, interval);
54+
pcl::toROSMsg(cloud, pcd_map_cell_with_id.pointcloud);
55+
res->new_pointcloud_with_ids.push_back(pcd_map_cell_with_id);
56+
res->header.frame_id = "map";
57+
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
58+
return true;
59+
}
60+
};
61+
62+
#endif // NDT_SCAN_MATCHER__TEST__STUB_PCD_LOADER_HPP_

‎localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
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 NDT_SCAN_MATCHER__TEST__STUB_SENSOR_PCD_PUBLISHER_HPP_
16+
#define NDT_SCAN_MATCHER__TEST__STUB_SENSOR_PCD_PUBLISHER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
21+
22+
class StubSensorPcdPublisher : public rclcpp::Node
23+
{
24+
public:
25+
StubSensorPcdPublisher() : Node("stub_sensor_pcd_publisher")
26+
{
27+
pcd_publisher_ =
28+
this->create_publisher<sensor_msgs::msg::PointCloud2>("/points_raw", 10);
29+
}
30+
31+
void publish_pcd(const sensor_msgs::msg::PointCloud2 & pcd) { pcd_publisher_->publish(pcd); }
32+
33+
private:
34+
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
35+
};
36+
37+
#endif // NDT_SCAN_MATCHER__TEST__STUB_SENSOR_PCD_PUBLISHER_HPP_

‎localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp

+64
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
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 NDT_SCAN_MATCHER__TEST__STUB_TRIGGER_NODE_CLIENT_HPP_
16+
#define NDT_SCAN_MATCHER__TEST__STUB_TRIGGER_NODE_CLIENT_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <std_srvs/srv/set_bool.hpp>
21+
22+
class StubTriggerNodeClient : public rclcpp::Node
23+
{
24+
using SetBool = std_srvs::srv::SetBool;
25+
26+
public:
27+
StubTriggerNodeClient() : Node("stub_trigger_node_client")
28+
{
29+
align_service_client_ = this->create_client<SetBool>("/trigger_node_srv");
30+
}
31+
32+
bool send_trigger_node(const bool & trigger)
33+
{
34+
// wait for the service to be available
35+
while (!align_service_client_->wait_for_service(std::chrono::seconds(1))) {
36+
if (!rclcpp::ok()) {
37+
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service.");
38+
EXIT_FAILURE;
39+
}
40+
RCLCPP_INFO(this->get_logger(), "Waiting for service...");
41+
}
42+
43+
// send the request
44+
std::shared_ptr<SetBool::Request> request = std::make_shared<SetBool::Request>();
45+
request->data = trigger;
46+
auto result = align_service_client_->async_send_request(request);
47+
48+
// wait for the response
49+
std::future_status status = result.wait_for(std::chrono::seconds(0));
50+
while (status != std::future_status::ready) {
51+
if (!rclcpp::ok()) {
52+
EXIT_FAILURE;
53+
}
54+
status = result.wait_for(std::chrono::seconds(1));
55+
}
56+
57+
return result.get()->success;
58+
}
59+
60+
private:
61+
rclcpp::Client<SetBool>::SharedPtr align_service_client_;
62+
};
63+
64+
#endif // NDT_SCAN_MATCHER__TEST__STUB_TRIGGER_NODE_CLIENT_HPP_

0 commit comments

Comments
 (0)