test(hesai): add basic tests for Hesai's PTC protocol

Signed-off-by: Max SCHMELLER <[email protected]>

nebula_hw_interfaces/CMakeLists.txt

@@ -100,6 +100,7 @@ if(BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 
     find_package(ament_cmake_gtest REQUIRED)
+    find_package(ament_cmake_gmock REQUIRED)
 
     ament_add_gtest(test_udp
         test/common/test_udp.cpp
@@ -109,6 +110,22 @@ 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}
+        include
+        test)
+
+    target_link_libraries(hesai_test_ptc
+        nebula_hw_interfaces_hesai
+    )
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

nebula_hw_interfaces/package.xml

@@ -20,6 +20,7 @@
   <depend>ros2_socketcan</depend>
   <depend>velodyne_msgs</depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 

nebula_hw_interfaces/test/hesai/test_ptc.cpp

@@ -0,0 +1,115 @@
+// Copyright 2024 TIER IV, Inc.
+
+#include "hesai/test_ptc/ptc_test.hpp"
+#include "nebula_common/hesai/hesai_common.hpp"
+#include "nebula_common/nebula_common.hpp"
+
+#include <gmock/gmock-cardinalities.h>
+#include <gmock/gmock-spec-builders.h>
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+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 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<HesaiSensorConfiguration>(config);
+}
+
+TEST_P(PtcTest, ConnectionLifecycle)
+{
+  /* Constructor does not immediately connect, destructor closes socket */ {
+    auto tcp_sock_ptr = make_mock_tcp_socket();
+    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 = make_mock_tcp_socket();
+    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->SetSensorConfiguration(cfg);
+    hw_interface->InitializeTcpDriver();
+    hw_interface->FinalizeTcpDriver();
+  }
+}
+
+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();
+};

nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp

@@ -0,0 +1,50 @@
+// 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 "hesai/test_ptc/tcp_mock.hpp"
+#include "nebula_common/loggers/console_logger.hpp"
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <utility>
+
+namespace nebula::drivers
+{
+class PtcTest : public ::testing::TestWithParam<SensorModel>
+{
+protected:
+  void SetUp() override {}
+
+  void TearDown() override {}
+
+  static auto make_mock_tcp_socket() { return std::make_shared<connections::MockTcpSocket>(); }
+
+  static auto make_hw_interface(std::shared_ptr<connections::MockTcpSocket> tcp_socket)
+  {
+    auto model = GetParam();
+
+    auto logger = std::make_shared<loggers::ConsoleLogger>("HwInterface");
+
+    auto hw_interface = std::make_unique<HesaiHwInterface>(logger, std::move(tcp_socket));
+    hw_interface->SetTargetModel(hw_interface->NebulaModelToHesaiModelNo(model));
+    return hw_interface;
+  }
+};
+
+}  // namespace nebula::drivers

nebula_hw_interfaces/test/hesai/test_ptc/tcp_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 <gmock/gmock.h>
+
+#include <string>
+#include <vector>
+
+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<uint8_t> & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload,
+     completion_callback_t cb_completion),
+    (override));
+};
+
+}  // namespace nebula::drivers::connections