Seyond (tier4#167)

* 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]>

* feat: add support to set return mode

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

* feat: add support to set PTP profile

This implementation supports configuring IEEE1588v2 or gPTP only. PTP
domain, PTP transport type, and PTP switch type configuration are not
available.
Signed-off-by: Manato HIRABAYASHI <[email protected]>

---------

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

nebula_common/include/nebula_common/seyond/seyond_common.hpp

@@ -80,6 +80,16 @@ struct SeyondCalibrationConfiguration : public SeyondCalibrationConfigurationBas
 inline ReturnMode ReturnModeFromStringSeyond(
   const std::string & return_mode, const SensorModel & sensor_model)
 {
+  switch (sensor_model) {
+    case SensorModel::SEYOND_FALCON_KINETIC:
+    case SensorModel::SEYOND_ROBIN_W:
+      if (return_mode == "Strongest") return ReturnMode::STRONGEST;
+      if (return_mode == "Dual") return ReturnMode::DUAL;
+      if (return_mode == "DualStrongestLast") return ReturnMode::DUAL_STRONGEST_LAST;
+      break;
+    default:
+      break;
+  }
   return ReturnMode::UNKNOWN;
 }
 
@@ -89,6 +99,16 @@ inline ReturnMode ReturnModeFromStringSeyond(
 /// @return Corresponding ReturnMode
 inline ReturnMode ReturnModeFromIntSeyond(const int return_mode, const SensorModel & sensor_model)
 {
+  switch (sensor_model) {
+    case SensorModel::SEYOND_FALCON_KINETIC:
+    case SensorModel::SEYOND_ROBIN_W:
+      if (return_mode == 1) return ReturnMode::STRONGEST;
+      if (return_mode == 2) return ReturnMode::DUAL;
+      if (return_mode == 3) return ReturnMode::DUAL_STRONGEST_LAST;
+      break;
+    default:
+      break;
+  }
   return ReturnMode::UNKNOWN;
 }
 
@@ -98,11 +118,20 @@ inline ReturnMode ReturnModeFromIntSeyond(const int return_mode, const SensorMod
 /// @return Corresponding return mode number for the hardware
 inline int IntFromReturnModeSeyond(const ReturnMode return_mode, const SensorModel & sensor_model)
 {
-
+  switch (sensor_model) {
+    case SensorModel::SEYOND_FALCON_KINETIC:
+    case SensorModel::SEYOND_ROBIN_W:
+      if (return_mode == ReturnMode::STRONGEST) return 1;
+      if (return_mode == ReturnMode::DUAL) return 2;
+      if (return_mode == ReturnMode::DUAL_STRONGEST_LAST) return 3;
+      break;
+    default:
+      break;
+  }
   return -1;
 }
 
 }  // namespace drivers
 }  // namespace nebula
 
-#endif  // NEBULA_SEYOND_COMMON_H
+#endif  // NEBULA_SEYOND_COMMON_H

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp

@@ -27,45 +27,60 @@
 #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();
 
   /// @brief Destructor
   ~SeyondHwInterface();
 
+  /// @brief Initializing tcp_driver for TCP communication
+  /// @return Resulting status
+  Status InitializeTcpDriver();
+
   /// @brief Callback function to receive the Cloud Packet data from the UDP Driver
   /// @param buffer Buffer containing the data received from the UDP socket
   void ReceiveSensorPacketCallback(std::vector<uint8_t> & buffer);
@@ -93,11 +108,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 +122,24 @@ 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();
+
+  /// @brief Setting device return mode
+  /// @param return_mode The mode of return
+  /// @return Resulting status
+  Status SetReturnMode(int return_mode);
+
+
+  /// @brief Setting PTP profile
+  /// @param profile profile to be set
+  /// @return Resulting status
+  Status SetPtpMode(PtpProfile profile);
+
+  /// @brief validate the current settings then set them
+  /// @return Resulting status
+  Status CheckAndSetConfig();
 };
 }  // namespace drivers
 }  // namespace nebula