feat: add initial seyond_hw_interface

This implementation has limited functions; just starting packet
streaming and enqueue them

Signed-off-by: Manato HIRABAYASHI <[email protected]>

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp

@@ -27,38 +27,49 @@
 #include <boost/property_tree/ptree.hpp>
 
 #include <memory>
-#include <mutex>
 #include <vector>
 
 namespace nebula
 {
 namespace drivers
 {
+constexpr uint16_t SeyondTcpCommandPort = 8001;
+constexpr uint16_t SeyondHttpCommandPort = 8010;
 /// @brief Hardware interface of seyond driver
 class SeyondHwInterface
 {
 private:
   std::unique_ptr<::drivers::common::IoContext> cloud_io_context_;
-  std::shared_ptr<boost::asio::io_context> m_owned_ctx;
   std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_;
   std::shared_ptr<const SeyondSensorConfiguration> sensor_configuration_;
   std::function<void(std::vector<uint8_t> & buffer)>
     cloud_packet_callback_; /**This function pointer is called when the scan is complete*/
 
-  int prev_phase_{};
-  int target_model_no;
+  std::shared_ptr<boost::asio::io_context> m_owned_ctx_;
+  std::unique_ptr<::drivers::tcp_driver::TcpDriver> command_tcp_driver_;
+
   std::shared_ptr<rclcpp::Logger> parent_node_logger_;
 
   /// @brief Printing the string to RCLCPP_INFO_STREAM
   /// @param info Target string
   void PrintInfo(std::string info);
+
   /// @brief Printing the string to RCLCPP_ERROR_STREAM
   /// @param error Target string
   void PrintError(std::string error);
+
   /// @brief Printing the string to RCLCPP_DEBUG_STREAM
   /// @param debug Target string
   void PrintDebug(std::string debug);
 
+  /// @brief Send TCP command to the device and return its response as string
+  /// @param commad payload of the command to be sent
+  /// @return string response from the device
+  std::string SendCommand(std::string commad);
+
+  /// @brief Start UDP stream from the device
+  Status StartUdpStreaming();
+
 public:
   /// @brief Constructor
   SeyondHwInterface();
@@ -93,11 +104,11 @@ class SeyondHwInterface
   /// @param sensor_configuration SensorConfiguration for this interface
   /// @return Resulting status
   Status SetSensorConfiguration(
-    std::shared_ptr<const SensorConfigurationBase> sensor_configuration);
+    const std::shared_ptr<const SensorConfigurationBase>& sensor_configuration);
 
-  /// @brief Set target model number
-  /// @param model Model number
-  void SetTargetModel(int model);
+  // /// @brief Set target model number
+  // /// @param model Model number
+  // void SetTargetModel(nebula::drivers::SensorModel model);
 
   /// @brief Registering callback for NebulaPackets
   /// @param scan_callback Callback function
@@ -107,6 +118,9 @@ class SeyondHwInterface
   /// @brief Setting rclcpp::Logger
   /// @param node Logger
   void SetLogger(std::shared_ptr<rclcpp::Logger> logger);
+
+  /// @brief Display common information acquired from sensor
+  void DisplayCommonVersion();
 };
 }  // namespace drivers
 }  // namespace nebula

nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp

@@ -1,21 +1,65 @@
 #include "nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp"
+
+#include <sstream>
+
 namespace nebula
 {
 namespace drivers
 {
 SeyondHwInterface::SeyondHwInterface()
 : cloud_io_context_{new ::drivers::common::IoContext(1)},
-  m_owned_ctx{new boost::asio::io_context(1)},
-  cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}
+  cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)},
+  sensor_configuration_(std::make_shared<const SeyondSensorConfiguration>()),
+  cloud_packet_callback_(nullptr),
+  m_owned_ctx_{new boost::asio::io_context(1)},
+  command_tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_)}
+{
+}
+
+SeyondHwInterface::~SeyondHwInterface()
 {
 }
 
 void SeyondHwInterface::ReceiveSensorPacketCallback(std::vector<uint8_t> & buffer)
 {
+  cloud_packet_callback_(buffer);
 }
 
 Status SeyondHwInterface::SensorInterfaceStart()
 {
+  StartUdpStreaming();
+
+  // Initialize command sender which communicate with the device via TCP
+  try {
+  command_tcp_driver_->init_socket(
+      sensor_configuration_->sensor_ip, SeyondTcpCommandPort, sensor_configuration_->host_ip,
+      SeyondTcpCommandPort);
+  } catch (const std::exception & ex) {
+    std::stringstream ss;
+    ss << "SeyondHwInterface::SensorInterfaceStart (init TCP): " << Status::ERROR_1
+       << std::endl;
+    PrintError(ss.str());
+    command_tcp_driver_->closeSync();
+    return Status::ERROR_1;
+  }
+
+  DisplayCommonVersion();
+
+  // Initialize stream receiver which communicate with the device via UDP
+  try {
+    std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl;
+    cloud_udp_driver_->init_receiver(
+        sensor_configuration_->host_ip, sensor_configuration_->data_port);
+    cloud_udp_driver_->receiver()->open();
+    cloud_udp_driver_->receiver()->bind();
+    cloud_udp_driver_->receiver()->asyncReceive(
+        std::bind(&SeyondHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1));
+  } catch (const std::exception & ex) {
+    Status status = Status::UDP_CONNECTION_ERROR;
+    std::cerr << status << sensor_configuration_->sensor_ip << ", "
+              << sensor_configuration_->data_port << std::endl;
+    return status;
+  }
   return Status::OK;
 }
 
@@ -25,50 +69,162 @@ Status SeyondHwInterface::SensorInterfaceStop()
 }
 
 Status SeyondHwInterface::SetSensorConfiguration(
-  std::shared_ptr<const SensorConfigurationBase> sensor_configuration)
+  const std::shared_ptr<const SensorConfigurationBase>& sensor_configuration)
 {
+  if (!(sensor_configuration->sensor_model == SensorModel::SEYOND_FALCON_KINETIC ||
+        sensor_configuration->sensor_model == SensorModel::SEYOND_ROBIN_W)) {
+    return Status::INVALID_SENSOR_MODEL;
+  }
+
+  sensor_configuration_ = std::static_pointer_cast<const SeyondSensorConfiguration>(sensor_configuration);
+
   return Status::OK;
 }
 
 Status SeyondHwInterface::GetSensorConfiguration(
   const SensorConfigurationBase & sensor_configuration)
 {
+  std::stringstream ss;
+  ss << sensor_configuration;
+  PrintDebug(ss.str());
   return Status::ERROR_1;
 }
 
 Status SeyondHwInterface::GetCalibrationConfiguration(
   const CalibrationConfigurationBase & calibration_configuration)
 {
+  PrintDebug(calibration_configuration.calibration_file);
   return Status::ERROR_1;
 }
 
 Status SeyondHwInterface::RegisterScanCallback(
   std::function<void(std::vector<uint8_t> &)> scan_callback)
 {
+  cloud_packet_callback_ = std::move(scan_callback);
   return Status::OK;
 }
 
-void SeyondHwInterface::SetTargetModel(int model)
-{
-  target_model_no = model;
-}
+// void SeyondHwInterface::SetTargetModel(nebula::drivers::SensorModel model)
+// {
+//   target_model_no = static_cast<int>(model);
+// }
 
 void SeyondHwInterface::PrintError(std::string error)
 {
+  if (parent_node_logger_) {
+    RCLCPP_ERROR_STREAM((*parent_node_logger_), error);
+  } else {
+    std::cerr << error << std::endl;
+  }
 }
 
 void SeyondHwInterface::PrintDebug(std::string debug)
 {
+  if (parent_node_logger_) {
+    RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug);
+  } else {
+    std::cout << debug << std::endl;
+  }
 }
 
 void SeyondHwInterface::PrintInfo(std::string info)
 {
+if (parent_node_logger_) {
+    RCLCPP_INFO_STREAM((*parent_node_logger_), info);
+  } else {
+    std::cout << info << std::endl;
+  }
 }
 
 void SeyondHwInterface::SetLogger(std::shared_ptr<rclcpp::Logger> logger)
 {
   parent_node_logger_ = logger;
 }
 
+void SeyondHwInterface::DisplayCommonVersion()
+{
+  std::stringstream info;
+  info << "*************** Innovusion Lidar Version Info ***************" <<std::endl;
+  info << "sw_version:" << SendCommand("get_version") << std::endl;
+  info << "sn:" << SendCommand("get_sn") << std::endl;
+  info << "framerate:" << SendCommand("get_i_config motor galvo_framerate") << std::endl;
+  info << "*************************************************************" << std::endl;
+  PrintDebug(info.str());
+}
+
+std::string SeyondHwInterface::SendCommand(std::string command)
+{
+  if (command_tcp_driver_->GetIOContext()->stopped()) {
+    command_tcp_driver_->GetIOContext()->restart();
+  }
+
+  std::vector<unsigned char> payload;
+  std::copy(command.begin(), command.end(), std::back_inserter(payload));
+  payload.emplace_back('\n');
+  payload.emplace_back(0);
+
+  command_tcp_driver_->asyncSend(payload);
+  command_tcp_driver_->run();
+
+  boost::asio::streambuf rec_buf;
+  command_tcp_driver_->socket()->receive(rec_buf);
+  std::string rec_str = boost::asio::buffer_cast<const char*>(rec_buf.data());
+  command_tcp_driver_->closeSync();
+  return rec_str;
+}
+
+Status SeyondHwInterface::StartUdpStreaming()
+{
+  // XXX: This streaming-starting command need to be sent via HTTP as of now
+  auto stream_starter_ctx = std::make_shared<boost::asio::io_context>(1);
+  auto stream_starter_http_driver = std::unique_ptr<::drivers::tcp_driver::HttpClientDriver>(
+      new ::drivers::tcp_driver::HttpClientDriver(stream_starter_ctx));
+
+  // Initialize stream starter which communicate with the device via HTTP
+  try {
+    stream_starter_http_driver->init_client(sensor_configuration_->sensor_ip, SeyondHttpCommandPort);
+  } catch (const std::exception & ex) {
+    std::stringstream ss;
+    ss << "SeyondHwInterface::SensorInterfaceStart (init HTTP): " << Status::HTTP_CONNECTION_ERROR
+       << std::endl;
+    PrintError(ss.str());
+    return Status::HTTP_CONNECTION_ERROR;
+  }
+
+  // XXX: Currently, one port is used to receive data, status, and message
+  uint16_t data_port = sensor_configuration_->data_port;
+  uint16_t status_port = sensor_configuration_->data_port;
+  uint16_t message_port = sensor_configuration_->data_port;
+  Status stat;
+  switch (sensor_configuration_->sensor_model)
+  {
+    case SensorModel::SEYOND_FALCON_KINETIC: {
+      std::string payload = std::to_string(data_port) + "," + std::to_string(message_port) + ","
+                            + std::to_string(status_port);
+      std::string http_command = "/command/?set_udp_ports_ip=" + payload;
+      auto response = stream_starter_http_driver->get(http_command);
+      PrintInfo(response);
+      stat = Status::OK;
+      break;
+    }
+    case SensorModel::SEYOND_ROBIN_W: {
+      std::string payload = "{\"ip\":\"" + sensor_configuration_->sensor_ip
+                            + "\",\"port_data\":" + std::to_string(data_port)
+                            + ",\"port_message\":" + std::to_string(message_port)
+                            + ",\"port_status\":" +  std::to_string(status_port) + "}";
+      stream_starter_http_driver->post( "/v1/lidar/force_udp_ports_ip", payload);
+      stat = Status::OK;
+      break;
+    }
+    default: {
+      PrintError("Invalid sensor model was specified");
+      stat = Status::INVALID_SENSOR_MODEL;
+    }
+  }
+  stream_starter_http_driver->client()->close();
+
+  return stat;
+}
+
 }  // namespace drivers
 }  // namespace nebula

nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp

@@ -20,6 +20,7 @@ class SeyondHwInterfaceWrapper
 public:
   SeyondHwInterfaceWrapper(
     rclcpp::Node * const parent_node, std::shared_ptr<const SeyondSensorConfiguration> & config);
+  SeyondHwInterfaceWrapper() = default;
 
   void OnConfigChange(const std::shared_ptr<const SeyondSensorConfiguration> & new_config);
 
@@ -28,10 +29,10 @@ class SeyondHwInterfaceWrapper
   std::shared_ptr<SeyondHwInterface> HwInterface() const;
 
 private:
-  std::shared_ptr<SeyondHwInterface> hw_interface_;
+  std::shared_ptr<SeyondHwInterface> hw_interface_{};
   rclcpp::Logger logger_;
-  nebula::Status status_;
-  bool setup_sensor_;
+  nebula::Status status_{};
+  bool setup_sensor_{};
 };
 }  // namespace ros
 }  // namespace nebula

nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp

@@ -26,6 +26,7 @@
 #include <mutex>
 #include <optional>
 #include <thread>
+#include <memory>
 
 namespace nebula
 {
@@ -57,19 +58,19 @@ class SeyondRosWrapper final : public rclcpp::Node
 
   Status ValidateAndSetConfig(std::shared_ptr<const SeyondSensorConfiguration> & new_config);
 
-  Status wrapper_status_;
+  Status wrapper_status_{};
 
   std::shared_ptr<const SeyondSensorConfiguration> sensor_cfg_ptr_{};
-  std::mutex mtx_config_;
+  std::mutex mtx_config_{};
 
   mt_queue<std::unique_ptr<nebula_msgs::msg::NebulaPacket>> packet_queue_;
 
   /// @brief Thread to isolate decoding from receiving
-  std::thread decoder_thread_;
+  std::thread decoder_thread_{};
 
   rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub_{};
 
-  bool launch_hw_;
+  bool launch_hw_{};
 
   std::optional<SeyondHwInterfaceWrapper> hw_interface_wrapper_;
   std::optional<SeyondHwMonitorWrapper> hw_monitor_wrapper_;