Skip to content

Commit b4eb3b5

Browse files
mojomexmojomex
committed
test(hesai): add unit tests for TCP socket and Hesai HW interface
Signed-off-by: Max SCHMELLER <max.schmeller@tier4.jp>
1 parent b9140fd commit b4eb3b5

12 files changed

Lines changed: 868 additions & 0 deletions

File tree

‎nebula_hw_interfaces/CMakeLists.txt‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,10 @@ if(BUILD_TESTING)
100100
ament_lint_auto_find_test_dependencies()
101101

102102
find_package(ament_cmake_gtest REQUIRED)
103+
find_package(ament_cmake_gmock REQUIRED)
104+
find_package(nlohmann_json)
105+
106+
add_definitions(-D_TEST_RESOURCES_PATH="${PROJECT_SOURCE_DIR}/test_resources/")
103107

104108
ament_add_gtest(test_udp
105109
test/common/test_udp.cpp
@@ -109,6 +113,24 @@ if(BUILD_TESTING)
109113
${nebula_common_INCLUDE_DIRS}
110114
include
111115
test)
116+
117+
ament_add_gmock(hesai_test_ptc
118+
test/hesai/test_ptc.cpp
119+
)
120+
121+
target_include_directories(hesai_test_ptc PUBLIC
122+
${nebula_common_INCLUDE_DIRS}
123+
${nebula_hw_interfaces_hesai_INCLUDE_DIRS}
124+
${boost_tcp_driver_INCLUDE_DIRS}
125+
${boost_udp_driver_INCLUDE_DIRS}
126+
${nlohmann_json_INCLUDE_DIRS}
127+
include
128+
test)
129+
130+
target_link_libraries(hesai_test_ptc
131+
${nlohmann_json_LIBRARIES}
132+
nebula_hw_interfaces_hesai
133+
)
112134
endif()
113135

114136
ament_export_include_directories("include/${PROJECT_NAME}")

‎nebula_hw_interfaces/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,10 @@
2020
<depend>ros2_socketcan</depend>
2121
<depend>velodyne_msgs</depend>
2222

23+
<test_depend>ament_cmake_gmock</test_depend>
2324
<test_depend>ament_cmake_gtest</test_depend>
2425
<test_depend>ament_lint_auto</test_depend>
26+
<test_depend>nlohmann-json-dev</test_depend>
2527

2628
<export>
2729
<build_type>ament_cmake</build_type>

‎nebula_hw_interfaces/test/hesai/test_ptc.cpp‎

Lines changed: 236 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,236 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
3+
#include "hesai/test_ptc/ptc_test.hpp"
4+
#include "hesai/test_ptc/tcp_socket_mock.hpp"
5+
#include "hesai/test_ptc/tcp_socket_replay.hpp"
6+
#include "nebula_common/hesai/hesai_common.hpp"
7+
#include "nebula_common/nebula_common.hpp"
8+
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
9+
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"
10+
11+
#include <gmock/gmock-cardinalities.h>
12+
#include <gmock/gmock-spec-builders.h>
13+
#include <gmock/gmock.h>
14+
#include <gtest/gtest.h>
15+
16+
#include <cmath>
17+
#include <exception>
18+
#include <filesystem>
19+
#include <iomanip>
20+
#include <memory>
21+
#include <sstream>
22+
23+
#ifndef _TEST_RESOURCES_PATH
24+
static_assert(false, "No test resources path defined");
25+
#endif
26+
27+
#define GTEST_SKIP_PRINT(x) \
28+
do { \
29+
std::cout << x << '\n'; \
30+
GTEST_SKIP() << x; \
31+
} while (0)
32+
33+
#define ASSERT_NO_THROW_PRINT(expr) \
34+
do { \
35+
try { \
36+
expr; \
37+
} catch (const std::exception & e) { \
38+
std::cout << e.what() << '\n'; \
39+
ASSERT_NO_THROW(throw e); \
40+
} \
41+
} while (0)
42+
43+
namespace nebula::drivers
44+
{
45+
46+
using testing::_;
47+
using testing::AtLeast;
48+
using testing::Exactly;
49+
using testing::InSequence;
50+
51+
const SensorModel g_models_under_test[] = {
52+
SensorModel::HESAI_PANDAR64, SensorModel::HESAI_PANDAR40P, SensorModel::HESAI_PANDARQT64,
53+
SensorModel::HESAI_PANDARQT128, SensorModel::HESAI_PANDARXT32, SensorModel::HESAI_PANDARAT128,
54+
SensorModel::HESAI_PANDAR128_E4X,
55+
};
56+
57+
const uint16_t g_u16_invalid = 0x4242;
58+
const uint16_t g_ptc_port = 9347;
59+
const size_t g_ptc_header_size = 8;
60+
const char g_host_ip[] = "192.168.42.42";
61+
const char g_sensor_ip[] = "192.168.84.84";
62+
63+
auto make_sensor_config(SensorModel model)
64+
{
65+
uint16_t rotation_speed = 600;
66+
uint16_t sync_angle = 0;
67+
double cut_angle = 0.0;
68+
uint16_t cloud_min_angle = 0;
69+
uint16_t cloud_max_angle = 360;
70+
71+
if (model == SensorModel::HESAI_PANDARAT128) {
72+
rotation_speed = 200;
73+
sync_angle = 30;
74+
cut_angle = 150.0;
75+
cloud_min_angle = 30;
76+
cloud_max_angle = 150;
77+
}
78+
79+
HesaiSensorConfiguration config{
80+
LidarConfigurationBase{
81+
EthernetSensorConfigurationBase{
82+
SensorConfigurationBase{model, "test"}, g_host_ip, g_sensor_ip, g_u16_invalid},
83+
ReturnMode::UNKNOWN,
84+
g_u16_invalid,
85+
g_u16_invalid,
86+
CoordinateMode::UNKNOWN,
87+
NAN,
88+
NAN,
89+
false,
90+
{},
91+
false},
92+
"",
93+
g_u16_invalid,
94+
sync_angle,
95+
cut_angle,
96+
0.1,
97+
"",
98+
rotation_speed,
99+
cloud_min_angle,
100+
cloud_max_angle,
101+
PtpProfile::IEEE_802_1AS_AUTO,
102+
0,
103+
PtpTransportType::L2,
104+
PtpSwitchType::NON_TSN};
105+
106+
return std::make_shared<HesaiSensorConfiguration>(config);
107+
}
108+
109+
TEST_P(PtcTest, ConnectionLifecycle)
110+
{
111+
/* Constructor does not immediately connect, destructor closes socket */ {
112+
auto tcp_sock_ptr = std::make_shared<connections::MockTcpSocket>();
113+
auto & tcp_sock = *tcp_sock_ptr;
114+
115+
EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1));
116+
auto hw_interface = make_hw_interface(tcp_sock_ptr);
117+
}
118+
119+
/* Full lifecycle without sending/receiving */ {
120+
auto tcp_sock_ptr = std::make_shared<connections::MockTcpSocket>();
121+
auto & tcp_sock = *tcp_sock_ptr;
122+
123+
InSequence seq;
124+
EXPECT_CALL(tcp_sock, init(g_host_ip, _, g_sensor_ip, g_ptc_port)).Times(Exactly(1));
125+
EXPECT_CALL(tcp_sock, bind()).Times(Exactly(1));
126+
EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1));
127+
128+
auto cfg = make_sensor_config(GetParam());
129+
130+
auto hw_interface = make_hw_interface(tcp_sock_ptr);
131+
hw_interface->SetSensorConfiguration(cfg);
132+
hw_interface->InitializeTcpDriver();
133+
hw_interface->FinalizeTcpDriver();
134+
}
135+
}
136+
137+
TEST_P(PtcTest, PtcCommunication)
138+
{
139+
const auto & model = GetParam();
140+
141+
// ////////////////////////////////////////
142+
// Set up database-based replay TCP socket
143+
// ////////////////////////////////////////
144+
145+
using ptc_handler_t = connections::ReplayTcpSocket::ptc_handler_t;
146+
using header_callback_t = connections::ReplayTcpSocket::header_callback_t;
147+
using payload_callback_t = connections::ReplayTcpSocket::payload_callback_t;
148+
using completion_callback_t = connections::ReplayTcpSocket::completion_callback_t;
149+
using connections::message_t;
150+
151+
auto conversation_db_path = std::filesystem::path(_TEST_RESOURCES_PATH) / "hesai" /
152+
(sensor_model_to_string(model) + ".json");
153+
if (!std::filesystem::exists(conversation_db_path)) {
154+
GTEST_SKIP_PRINT("conversation DB " << conversation_db_path << " does not exist");
155+
}
156+
157+
auto conversation_db_exp = connections::parse_conversation_db(conversation_db_path);
158+
if (!conversation_db_exp.has_value()) {
159+
std::cout << "ParseError: " << conversation_db_exp.error().what() << '\n';
160+
GTEST_SKIP_PRINT(
161+
"conversation DB for model "
162+
<< model << " could not be parsed due to ParseError: " << conversation_db_exp.error().what());
163+
}
164+
165+
connections::conversation_db_t conversation_db = conversation_db_exp.value();
166+
167+
ptc_handler_t cb = [&conversation_db](
168+
const message_t & request, const header_callback_t & cb_header,
169+
const payload_callback_t & cb_payload,
170+
const completion_callback_t & cb_completion) {
171+
if (conversation_db.find(request) == conversation_db.end()) {
172+
std::stringstream ss;
173+
ss << "0x";
174+
for (const uint8_t & byte : request) {
175+
ss << std::hex << std::setfill('0') << std::setw(2) << static_cast<int>(byte);
176+
}
177+
GTEST_SKIP_PRINT("request " << ss.str() << " not found in conversation DB");
178+
}
179+
180+
const auto & responses = conversation_db[request];
181+
182+
message_t complete_response;
183+
184+
for (const message_t & response : responses) {
185+
complete_response.insert(complete_response.end(), response.cbegin(), response.cend());
186+
}
187+
188+
auto header = message_t(
189+
complete_response.cbegin(), std::next(complete_response.cbegin(), g_ptc_header_size));
190+
cb_header(header);
191+
cb_payload(complete_response);
192+
cb_completion();
193+
};
194+
195+
auto tcp_sock_ptr = std::make_shared<connections::ReplayTcpSocket>(std::move(cb));
196+
197+
// ////////////////////////////////////////
198+
// Test HW interface
199+
// ////////////////////////////////////////
200+
201+
auto hw_interface = make_hw_interface(tcp_sock_ptr);
202+
203+
auto cfg = make_sensor_config(GetParam());
204+
hw_interface->SetSensorConfiguration(cfg);
205+
hw_interface->InitializeTcpDriver();
206+
207+
// ////////////////////////////////////////
208+
// Applicable to all models
209+
// ////////////////////////////////////////
210+
211+
std::shared_ptr<HesaiConfigBase> config;
212+
ASSERT_NO_THROW_PRINT(config = hw_interface->GetConfig());
213+
ASSERT_NE(config, nullptr);
214+
215+
std::shared_ptr<HesaiInventoryBase> inventory;
216+
ASSERT_NO_THROW_PRINT(inventory = hw_interface->GetInventory());
217+
ASSERT_NE(inventory, nullptr);
218+
219+
std::vector<uint8_t> calibration;
220+
ASSERT_NO_THROW_PRINT(calibration = hw_interface->GetLidarCalibrationBytes());
221+
ASSERT_FALSE(calibration.empty());
222+
223+
std::shared_ptr<HesaiLidarStatusBase> status;
224+
ASSERT_NO_THROW_PRINT(status = hw_interface->GetLidarStatus());
225+
ASSERT_NE(status, nullptr);
226+
}
227+
228+
INSTANTIATE_TEST_SUITE_P(TestMain, PtcTest, testing::ValuesIn(g_models_under_test));
229+
230+
} // namespace nebula::drivers
231+
232+
int main(int argc, char * argv[])
233+
{
234+
::testing::InitGoogleTest(&argc, argv);
235+
return RUN_ALL_TESTS();
236+
};

‎nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
// Copyright 2024 TIER IV, Inc.
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+
#pragma once
16+
17+
#include "nebula_common/loggers/console_logger.hpp"
18+
#include "nebula_common/nebula_common.hpp"
19+
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
20+
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
21+
22+
#include <gtest/gtest.h>
23+
24+
#include <memory>
25+
#include <utility>
26+
27+
namespace nebula::drivers
28+
{
29+
class PtcTest : public ::testing::TestWithParam<SensorModel>
30+
{
31+
protected:
32+
void SetUp() override { std::cout << "GetParam() = " << GetParam() << '\n'; }
33+
34+
void TearDown() override {}
35+
36+
static auto make_hw_interface(std::shared_ptr<connections::AbstractTcpSocket> tcp_socket)
37+
{
38+
auto model = GetParam();
39+
40+
auto logger = std::make_shared<loggers::ConsoleLogger>("HwInterface");
41+
42+
auto hw_interface = std::make_unique<HesaiHwInterface>(logger, std::move(tcp_socket));
43+
hw_interface->SetTargetModel(hw_interface->NebulaModelToHesaiModelNo(model));
44+
return hw_interface;
45+
}
46+
};
47+
48+
} // namespace nebula::drivers

‎nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_mock.hpp‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright 2024 TIER IV, Inc.
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+
#pragma once
16+
17+
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
18+
19+
#include <gmock/gmock.h>
20+
21+
#include <string>
22+
#include <vector>
23+
24+
namespace nebula::drivers::connections
25+
{
26+
27+
class MockTcpSocket : public AbstractTcpSocket
28+
{
29+
public:
30+
MOCK_METHOD(
31+
void, init,
32+
(const std::string & host_ip, uint16_t host_port, const std::string & remote_ip,
33+
uint16_t remote_port),
34+
(override));
35+
36+
MOCK_METHOD(void, bind, (), (override));
37+
38+
MOCK_METHOD(void, close, (), (override));
39+
40+
MOCK_METHOD(
41+
void, async_ptc_request,
42+
(std::vector<uint8_t> & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload,
43+
completion_callback_t cb_completion),
44+
(override));
45+
};
46+
47+
} // namespace nebula::drivers::connections

0 commit comments

Comments
 (0)