diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt
index f5dc6569b..42b196394 100644
--- a/nebula_hw_interfaces/CMakeLists.txt
+++ b/nebula_hw_interfaces/CMakeLists.txt
@@ -100,6 +100,10 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
+ find_package(ament_cmake_gmock REQUIRED)
+ find_package(nlohmann_json)
+
+ add_definitions(-D_TEST_RESOURCES_PATH="${PROJECT_SOURCE_DIR}/test_resources/")
ament_add_gtest(test_udp
test/common/test_udp.cpp
@@ -109,6 +113,24 @@ if(BUILD_TESTING)
${nebula_common_INCLUDE_DIRS}
include
test)
+
+ ament_add_gmock(hesai_test_ptc
+ test/hesai/test_ptc.cpp
+ )
+
+ target_include_directories(hesai_test_ptc PUBLIC
+ ${nebula_common_INCLUDE_DIRS}
+ ${nebula_hw_interfaces_hesai_INCLUDE_DIRS}
+ ${boost_tcp_driver_INCLUDE_DIRS}
+ ${boost_udp_driver_INCLUDE_DIRS}
+ ${nlohmann_json_INCLUDE_DIRS}
+ include
+ test)
+
+ target_link_libraries(hesai_test_ptc
+ ${nlohmann_json_LIBRARIES}
+ nebula_hw_interfaces_hesai
+ )
endif()
ament_export_include_directories("include/${PROJECT_NAME}")
diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml
index f77e88765..48a7413ff 100644
--- a/nebula_hw_interfaces/package.xml
+++ b/nebula_hw_interfaces/package.xml
@@ -20,8 +20,10 @@
ros2_socketcan
velodyne_msgs
+ ament_cmake_gmock
ament_cmake_gtest
ament_lint_auto
+ nlohmann-json-dev
ament_cmake
diff --git a/nebula_hw_interfaces/test/hesai/test_ptc.cpp b/nebula_hw_interfaces/test/hesai/test_ptc.cpp
new file mode 100644
index 000000000..0f388dcff
--- /dev/null
+++ b/nebula_hw_interfaces/test/hesai/test_ptc.cpp
@@ -0,0 +1,236 @@
+// Copyright 2024 TIER IV, Inc.
+
+#include "hesai/test_ptc/ptc_test.hpp"
+#include "hesai/test_ptc/tcp_socket_mock.hpp"
+#include "hesai/test_ptc/tcp_socket_replay.hpp"
+#include "nebula_common/hesai/hesai_common.hpp"
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#ifndef _TEST_RESOURCES_PATH
+static_assert(false, "No test resources path defined");
+#endif
+
+#define GTEST_SKIP_PRINT(x) \
+ do { \
+ std::cout << x << '\n'; \
+ GTEST_SKIP() << x; \
+ } while (0)
+
+#define ASSERT_NO_THROW_PRINT(expr) \
+ do { \
+ try { \
+ expr; \
+ } catch (const std::exception & e) { \
+ std::cout << e.what() << '\n'; \
+ ASSERT_NO_THROW(throw e); \
+ } \
+ } while (0)
+
+namespace nebula::drivers
+{
+
+using testing::_;
+using testing::AtLeast;
+using testing::Exactly;
+using testing::InSequence;
+
+const SensorModel g_models_under_test[] = {
+ SensorModel::HESAI_PANDAR64, SensorModel::HESAI_PANDAR40P, SensorModel::HESAI_PANDARQT64,
+ SensorModel::HESAI_PANDARQT128, SensorModel::HESAI_PANDARXT32, SensorModel::HESAI_PANDARAT128,
+ SensorModel::HESAI_PANDAR128_E4X,
+};
+
+const uint16_t g_u16_invalid = 0x4242;
+const uint16_t g_ptc_port = 9347;
+const size_t g_ptc_header_size = 8;
+const char g_host_ip[] = "192.168.42.42";
+const char g_sensor_ip[] = "192.168.84.84";
+
+auto make_sensor_config(SensorModel model)
+{
+ uint16_t rotation_speed = 600;
+ uint16_t sync_angle = 0;
+ double cut_angle = 0.0;
+ uint16_t cloud_min_angle = 0;
+ uint16_t cloud_max_angle = 360;
+
+ if (model == SensorModel::HESAI_PANDARAT128) {
+ rotation_speed = 200;
+ sync_angle = 30;
+ cut_angle = 150.0;
+ cloud_min_angle = 30;
+ cloud_max_angle = 150;
+ }
+
+ HesaiSensorConfiguration config{
+ LidarConfigurationBase{
+ EthernetSensorConfigurationBase{
+ SensorConfigurationBase{model, "test"}, g_host_ip, g_sensor_ip, g_u16_invalid},
+ ReturnMode::UNKNOWN,
+ g_u16_invalid,
+ g_u16_invalid,
+ CoordinateMode::UNKNOWN,
+ NAN,
+ NAN,
+ false,
+ {},
+ false},
+ "",
+ g_u16_invalid,
+ sync_angle,
+ cut_angle,
+ 0.1,
+ "",
+ rotation_speed,
+ cloud_min_angle,
+ cloud_max_angle,
+ PtpProfile::IEEE_802_1AS_AUTO,
+ 0,
+ PtpTransportType::L2,
+ PtpSwitchType::NON_TSN};
+
+ return std::make_shared(config);
+}
+
+TEST_P(PtcTest, ConnectionLifecycle)
+{
+ /* Constructor does not immediately connect, destructor closes socket */ {
+ auto tcp_sock_ptr = std::make_shared();
+ auto & tcp_sock = *tcp_sock_ptr;
+
+ EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1));
+ auto hw_interface = make_hw_interface(tcp_sock_ptr);
+ }
+
+ /* Full lifecycle without sending/receiving */ {
+ auto tcp_sock_ptr = std::make_shared();
+ auto & tcp_sock = *tcp_sock_ptr;
+
+ InSequence seq;
+ EXPECT_CALL(tcp_sock, init(g_host_ip, _, g_sensor_ip, g_ptc_port)).Times(Exactly(1));
+ EXPECT_CALL(tcp_sock, bind()).Times(Exactly(1));
+ EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1));
+
+ auto cfg = make_sensor_config(GetParam());
+
+ auto hw_interface = make_hw_interface(tcp_sock_ptr);
+ hw_interface->set_sensor_configuration(cfg);
+ hw_interface->initialize_tcp_driver();
+ hw_interface->finalize_tcp_driver();
+ }
+}
+
+TEST_P(PtcTest, PtcCommunication)
+{
+ const auto & model = GetParam();
+
+ // ////////////////////////////////////////
+ // Set up database-based replay TCP socket
+ // ////////////////////////////////////////
+
+ using ptc_handler_t = connections::ReplayTcpSocket::ptc_handler_t;
+ using header_callback_t = connections::ReplayTcpSocket::header_callback_t;
+ using payload_callback_t = connections::ReplayTcpSocket::payload_callback_t;
+ using completion_callback_t = connections::ReplayTcpSocket::completion_callback_t;
+ using connections::message_t;
+
+ auto conversation_db_path = std::filesystem::path(_TEST_RESOURCES_PATH) / "hesai" /
+ (sensor_model_to_string(model) + ".json");
+ if (!std::filesystem::exists(conversation_db_path)) {
+ GTEST_SKIP_PRINT("conversation DB " << conversation_db_path << " does not exist");
+ }
+
+ auto conversation_db_exp = connections::parse_conversation_db(conversation_db_path);
+ if (!conversation_db_exp.has_value()) {
+ std::cout << "ParseError: " << conversation_db_exp.error().what() << '\n';
+ GTEST_SKIP_PRINT(
+ "conversation DB for model "
+ << model << " could not be parsed due to ParseError: " << conversation_db_exp.error().what());
+ }
+
+ connections::conversation_db_t conversation_db = conversation_db_exp.value();
+
+ ptc_handler_t cb = [&conversation_db](
+ const message_t & request, const header_callback_t & cb_header,
+ const payload_callback_t & cb_payload,
+ const completion_callback_t & cb_completion) {
+ if (conversation_db.find(request) == conversation_db.end()) {
+ std::stringstream ss;
+ ss << "0x";
+ for (const uint8_t & byte : request) {
+ ss << std::hex << std::setfill('0') << std::setw(2) << static_cast(byte);
+ }
+ GTEST_SKIP_PRINT("request " << ss.str() << " not found in conversation DB");
+ }
+
+ const auto & responses = conversation_db[request];
+
+ message_t complete_response;
+
+ for (const message_t & response : responses) {
+ complete_response.insert(complete_response.end(), response.cbegin(), response.cend());
+ }
+
+ auto header = message_t(
+ complete_response.cbegin(), std::next(complete_response.cbegin(), g_ptc_header_size));
+ cb_header(header);
+ cb_payload(complete_response);
+ cb_completion();
+ };
+
+ auto tcp_sock_ptr = std::make_shared(std::move(cb));
+
+ // ////////////////////////////////////////
+ // Test HW interface
+ // ////////////////////////////////////////
+
+ auto hw_interface = make_hw_interface(tcp_sock_ptr);
+
+ auto cfg = make_sensor_config(GetParam());
+ hw_interface->set_sensor_configuration(cfg);
+ hw_interface->initialize_tcp_driver();
+
+ // ////////////////////////////////////////
+ // Applicable to all models
+ // ////////////////////////////////////////
+
+ std::shared_ptr config;
+ ASSERT_NO_THROW_PRINT(config = hw_interface->get_config());
+ ASSERT_NE(config, nullptr);
+
+ std::shared_ptr inventory;
+ ASSERT_NO_THROW_PRINT(inventory = hw_interface->get_inventory());
+ ASSERT_NE(inventory, nullptr);
+
+ std::vector calibration;
+ ASSERT_NO_THROW_PRINT(calibration = hw_interface->get_lidar_calibration_bytes());
+ ASSERT_FALSE(calibration.empty());
+
+ std::shared_ptr status;
+ ASSERT_NO_THROW_PRINT(status = hw_interface->get_lidar_status());
+ ASSERT_NE(status, nullptr);
+}
+
+INSTANTIATE_TEST_SUITE_P(TestMain, PtcTest, testing::ValuesIn(g_models_under_test));
+
+} // namespace nebula::drivers
+
+int main(int argc, char * argv[])
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+};
diff --git a/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp b/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp
new file mode 100644
index 000000000..d329f949c
--- /dev/null
+++ b/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp
@@ -0,0 +1,48 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#pragma once
+
+#include "nebula_common/loggers/console_logger.hpp"
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
+
+#include
+
+#include
+#include
+
+namespace nebula::drivers
+{
+class PtcTest : public ::testing::TestWithParam
+{
+protected:
+ void SetUp() override { std::cout << "GetParam() = " << GetParam() << '\n'; }
+
+ void TearDown() override {}
+
+ static auto make_hw_interface(std::shared_ptr tcp_socket)
+ {
+ auto model = GetParam();
+
+ auto logger = std::make_shared("HwInterface");
+
+ auto hw_interface = std::make_unique(logger, std::move(tcp_socket));
+ hw_interface->set_target_model(hw_interface->nebula_model_to_hesai_model_no(model));
+ return hw_interface;
+ }
+};
+
+} // namespace nebula::drivers
diff --git a/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_mock.hpp b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_mock.hpp
new file mode 100644
index 000000000..3916c13f2
--- /dev/null
+++ b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_mock.hpp
@@ -0,0 +1,47 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#pragma once
+
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
+
+#include
+
+#include
+#include
+
+namespace nebula::drivers::connections
+{
+
+class MockTcpSocket : public AbstractTcpSocket
+{
+public:
+ MOCK_METHOD(
+ void, init,
+ (const std::string & host_ip, uint16_t host_port, const std::string & remote_ip,
+ uint16_t remote_port),
+ (override));
+
+ MOCK_METHOD(void, bind, (), (override));
+
+ MOCK_METHOD(void, close, (), (override));
+
+ MOCK_METHOD(
+ void, async_ptc_request,
+ (std::vector & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload,
+ completion_callback_t cb_completion),
+ (override));
+};
+
+} // namespace nebula::drivers::connections
diff --git a/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_replay.hpp b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_replay.hpp
new file mode 100644
index 000000000..bbd6f07a6
--- /dev/null
+++ b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_socket_replay.hpp
@@ -0,0 +1,154 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#pragma once
+
+#include "nebula_common/util/expected.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include