test(hesai): dependency-injectable TCP socket, HW interface testability

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}")

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp

@@ -0,0 +1,103 @@
+// 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 <boost_tcp_driver/tcp_driver.hpp>
+
+#include <boost/asio/io_context.hpp>
+
+#include <cstdint>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace nebula::drivers::connections
+{
+
+class TcpError : public std::runtime_error
+{
+public:
+  explicit TcpError(const std::string & msg) : std::runtime_error(msg) {}
+};
+
+class AbstractTcpSocket
+{
+public:
+  using header_callback_t = std::function<void(const std::vector<uint8_t> &)>;
+  using payload_callback_t = std::function<void(const std::vector<uint8_t> &)>;
+  using completion_callback_t = std::function<void()>;
+
+  virtual ~AbstractTcpSocket() = default;
+
+  virtual void init(
+    const std::string & host_ip, uint16_t host_port, const std::string & remote_ip,
+    uint16_t remote_port) = 0;
+
+  virtual void bind() = 0;
+
+  virtual void close() = 0;
+
+  virtual 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) = 0;
+};
+
+class TcpSocket : public AbstractTcpSocket
+{
+public:
+  using callback_t = std::function<void(const std::vector<uint8_t> &)>;
+
+  void init(
+    const std::string & host_ip, uint16_t host_port, const std::string & remote_ip,
+    uint16_t remote_port) override
+  {
+    tcp_driver_.init_socket(remote_ip, remote_port, host_ip, host_port);
+  }
+
+  void bind() override
+  {
+    if (!tcp_driver_.isOpen() && !tcp_driver_.open()) {
+      throw TcpError("could not open TCP socket for an unknown reason");
+    }
+  }
+
+  void close() override
+  {
+    if (tcp_driver_.isOpen()) {
+      tcp_driver_.close();
+    }
+  }
+
+  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
+  {
+    if (tcp_driver_.GetIOContext()->stopped()) {
+      tcp_driver_.GetIOContext()->restart();
+    }
+    bool success =
+      tcp_driver_.asyncSendReceiveHeaderPayload(ptc_packet, cb_header, cb_payload, cb_completion);
+    if (!success) {
+      throw TcpError("sending the PTC command failed for an unknown reason");
+    }
+    tcp_driver_.GetIOContext()->run();
+  }
+
+private:
+  ::drivers::tcp_driver::TcpDriver tcp_driver_{std::make_shared<boost::asio::io_context>(1)};
+};
+
+}  // namespace nebula::drivers::connections

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -18,6 +18,7 @@
 // boost/property_tree/ in some versions of boost.
 // See: https://github.com/boostorg/property_tree/issues/51
 #include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp"
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp"
 
 #include <nebula_common/nebula_status.hpp>
 
@@ -33,7 +34,6 @@
 #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"
 
 #include <boost_tcp_driver/http_client_driver.hpp>
-#include <boost_tcp_driver/tcp_driver.hpp>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/hesai/hesai_status.hpp>
 #include <nebula_common/loggers/logger.hpp>
@@ -130,8 +130,7 @@ class HesaiHwInterface
 
   std::shared_ptr<loggers::Logger> logger_;
   std::optional<connections::UdpSocket> udp_socket_;
-  std::shared_ptr<boost::asio::io_context> m_owned_ctx_;
-  std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_;
+  std::shared_ptr<connections::AbstractTcpSocket> tcp_socket_;
   std::shared_ptr<const HesaiSensorConfiguration> sensor_configuration_;
   std::function<void(const std::vector<uint8_t> & buffer)>
     cloud_packet_callback_; /**This function pointer is called when the scan is complete*/
@@ -181,7 +180,9 @@ class HesaiHwInterface
 
 public:
   /// @brief Constructor
-  explicit HesaiHwInterface(const std::shared_ptr<loggers::Logger> & logger);
+  HesaiHwInterface(
+    const std::shared_ptr<loggers::Logger> & logger,
+    std::shared_ptr<connections::AbstractTcpSocket> tcp_socket);
   /// @brief Destructor
   ~HesaiHwInterface();
   /// @brief Initializing tcp_driver for TCP communication
@@ -360,12 +361,6 @@ class HesaiHwInterface
   /// @return Resulting status
   HesaiLidarMonitor get_lidar_monitor();
 
-  /// @brief Call run() of IO Context
-  void io_context_run();
-  /// @brief GetIO Context
-  /// @return IO Context
-  std::shared_ptr<boost::asio::io_context> get_io_context();
-
   /// @brief Setting spin_speed via HTTP API
   /// @param ctx IO Context used
   /// @param rpm spin_speed (300, 600, 1200)

nebula_hw_interfaces/package.xml

@@ -20,8 +20,10 @@
   <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>
+  <test_depend>nlohmann-json-dev</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -42,10 +42,11 @@ namespace nebula::drivers
 using std::string_literals::operator""s;
 using nlohmann::json;
 
-HesaiHwInterface::HesaiHwInterface(const std::shared_ptr<loggers::Logger> & logger)
+HesaiHwInterface::HesaiHwInterface(
+  const std::shared_ptr<loggers::Logger> & logger,
+  std::shared_ptr<connections::AbstractTcpSocket> tcp_socket)
 : logger_(logger),
-  m_owned_ctx_{new boost::asio::io_context(1)},
-  tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_)},
+  tcp_socket_{std::move(tcp_socket)},
   target_model_no_(nebula_model_to_hesai_model_no(SensorModel::UNKNOWN))
 {
 }
@@ -90,13 +91,8 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::send_receive(
   std::timed_mutex tm;
   tm.lock();
 
-  if (tcp_driver_->GetIOContext()->stopped()) {
-    logger_->debug(log_tag + "IOContext was stopped");
-    tcp_driver_->GetIOContext()->restart();
-  }
-
   logger_->debug(log_tag + "Sending payload");
-  tcp_driver_->asyncSendReceiveHeaderPayload(
+  tcp_socket_->async_ptc_request(
     send_buf,
     [this, log_tag, command_id, response_complete,
      error_code](const std::vector<uint8_t> & header_bytes) {
@@ -136,7 +132,6 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::send_receive(
       tm.unlock();
       logger_->debug(log_tag + "Unlocked mutex");
     });
-  this->io_context_run();
   if (!tm.try_lock_for(std::chrono::seconds(1))) {
     logger_->error(log_tag + "Request did not finish within 1s");
     error_code->error_flags |= g_tcp_error_timeout;
@@ -233,40 +228,16 @@ Status HesaiHwInterface::get_calibration_configuration(
 
 Status HesaiHwInterface::initialize_tcp_driver()
 {
-#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
-  std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl;
-  std::cout << "st: tcp_driver_->init_socket" << std::endl;
-  std::cout << "sensor_configuration_->sensor_ip=" << sensor_configuration_->sensor_ip << std::endl;
-  std::cout << "sensor_configuration_->host_ip=" << sensor_configuration_->host_ip << std::endl;
-  std::cout << "PandarTcpCommandPort=" << PandarTcpCommandPort << std::endl;
-#endif
-  tcp_driver_->init_socket(
-    sensor_configuration_->sensor_ip, g_pandar_tcp_command_port, sensor_configuration_->host_ip,
+  tcp_socket_->init(
+    sensor_configuration_->host_ip, g_pandar_tcp_command_port, sensor_configuration_->sensor_ip,
     g_pandar_tcp_command_port);
-#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
-  std::cout << "ed: tcp_driver_->init_socket" << std::endl;
-#endif
-  if (!tcp_driver_->open()) {
-#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
-    std::cout << "!tcp_driver_->open()" << std::endl;
-#endif
-    //    tcp_driver_->close();
-    tcp_driver_->closeSync();
-    return Status::ERROR_1;
-  }
+  tcp_socket_->bind();
   return Status::OK;
 }
 
 Status HesaiHwInterface::finalize_tcp_driver()
 {
-  try {
-    if (tcp_driver_) {
-      tcp_driver_->close();
-    }
-  } catch (std::exception & e) {
-    logger_->error("Error while finalizing the TcpDriver");
-    return Status::UDP_CONNECTION_ERROR;
-  }
+  tcp_socket_->close();
   return Status::OK;
 }
 
@@ -739,16 +710,6 @@ HesaiLidarMonitor HesaiHwInterface::get_lidar_monitor()
   return check_size_and_parse<HesaiLidarMonitor>(response);
 }
 
-void HesaiHwInterface::io_context_run()
-{
-  m_owned_ctx_->run();
-}
-
-std::shared_ptr<boost::asio::io_context> HesaiHwInterface::get_io_context()
-{
-  return m_owned_ctx_;
-}
-
 HesaiStatus HesaiHwInterface::get_http_client_driver_once(
   std::shared_ptr<boost::asio::io_context> ctx,
   std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd)