feat: add support to set return mode

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

@@ -77,6 +77,10 @@ class 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);
@@ -121,6 +125,15 @@ class SeyondHwInterface
 
   /// @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 validate the current settings then set them
+  /// @return Resulting status
+  Status CheckAndSetConfig();
 };
 }  // namespace drivers
 }  // namespace nebula

nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp

@@ -20,20 +20,13 @@ SeyondHwInterface::~SeyondHwInterface()
 {
 }
 
-void SeyondHwInterface::ReceiveSensorPacketCallback(std::vector<uint8_t> & buffer)
-{
-  cloud_packet_callback_(buffer);
-}
-
-Status SeyondHwInterface::SensorInterfaceStart()
+Status SeyondHwInterface::InitializeTcpDriver()
 {
-  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);
+    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
@@ -44,6 +37,17 @@ Status SeyondHwInterface::SensorInterfaceStart()
   }
 
   DisplayCommonVersion();
+  return Status::OK;
+}
+
+void SeyondHwInterface::ReceiveSensorPacketCallback(std::vector<uint8_t> & buffer)
+{
+  cloud_packet_callback_(buffer);
+}
+
+Status SeyondHwInterface::SensorInterfaceStart()
+{
+  StartUdpStreaming();
 
   // Initialize stream receiver which communicate with the device via UDP
   try {
@@ -203,7 +207,6 @@ Status SeyondHwInterface::StartUdpStreaming()
                             + 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;
     }
@@ -226,5 +229,33 @@ Status SeyondHwInterface::StartUdpStreaming()
   return stat;
 }
 
+Status SeyondHwInterface::SetReturnMode(int return_mode)
+{
+  std::string command = "set_i_config manufacture multiple_return_mode " + std::to_string(return_mode);
+  SendCommand(command);
+  return Status::OK;
+}
+
+Status SeyondHwInterface::CheckAndSetConfig()
+{
+  Status ret = Status::ERROR_1;
+  // Set return mode
+  auto return_mode_int = nebula::drivers::IntFromReturnModeSeyond(
+      sensor_configuration_->return_mode, sensor_configuration_->sensor_model);
+  if (return_mode_int < 0) {
+    PrintError(
+        "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual "
+        "mode.");
+    return_mode_int = nebula::drivers::IntFromReturnModeSeyond(ReturnMode::DUAL,
+                                                               sensor_configuration_->sensor_model);
+
+  }
+  ret = SetReturnMode(return_mode_int);
+  if (ret != Status::OK) {
+    return ret;
+  }
+  return Status::OK;
+}
+
 }  // namespace drivers
 }  // namespace nebula

nebula_ros/src/seyond/hw_interface_wrapper.cpp

@@ -13,7 +13,7 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper(
   setup_sensor_(false)
 {
   setup_sensor_ = parent_node->declare_parameter<bool>("setup_sensor", true, param_read_only());
-  //bool retry_connect = parent_node->declare_parameter<bool>("retry_hw", true, param_read_only());
+  bool retry_connect = parent_node->declare_parameter<bool>("retry_hw", true, param_read_only());
 
   status_ = hw_interface_->SetSensorConfiguration(
       std::static_pointer_cast<const drivers::SensorConfigurationBase>(config));
@@ -29,18 +29,24 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper(
   int retry_count = 0;
 
   // NOTE: for when TP interface is implemented
-  // while (true)
-  // {
-  //   status_ = hw_interface_->InitializeTcpDriver();
-  //   if (status_ == Status::OK || !retry_connect)
-  //   {
-  //     break;
-  //   }
+  while (true)
+  {
+    status_ = hw_interface_->InitializeTcpDriver();
+    if (status_ == Status::OK || !retry_connect)
+    {
+      break;
+    }
 
-  //   retry_count++;
-  //   std::this_thread::sleep_for(std::chrono::milliseconds(8000));  // >5000
-  //   RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count);
-  // }
+    retry_count++;
+    std::this_thread::sleep_for(std::chrono::milliseconds(8000));  // >5000
+    RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count);
+  }
+
+  if (status_ == Status::OK) {
+    if (setup_sensor_) {
+      hw_interface_->CheckAndSetConfig();
+    }
+  }
 
   status_ = Status::OK;
 }
@@ -50,9 +56,9 @@ void SeyondHwInterfaceWrapper::OnConfigChange(
 {
   hw_interface_->SetSensorConfiguration(
       std::static_pointer_cast<const drivers::SensorConfigurationBase>(new_config));
-  // if (setup_sensor_) {
-  //   hw_interface_->CheckAndSetConfig();
-  // }
+  if (setup_sensor_) {
+    hw_interface_->CheckAndSetConfig();
+  }
 }
 
 Status SeyondHwInterfaceWrapper::Status()

nebula_ros/src/seyond/seyond_ros_wrapper.cpp

@@ -88,7 +88,7 @@ nebula::Status SeyondRosWrapper::DeclareAndGetSensorConfigParams()
   config.sensor_model = drivers::SensorModelFromString(_sensor_model);
 
   auto _return_mode = declare_parameter<std::string>("return_mode", "", param_read_write());
-  config.return_mode = drivers::ReturnModeFromString(_return_mode);
+  config.return_mode = drivers::ReturnModeFromStringSeyond(_return_mode, config.sensor_model);
 
   config.host_ip = declare_parameter<std::string>("host_ip", "255.255.255.255", param_read_only());
   config.sensor_ip =
@@ -276,7 +276,8 @@ rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(
   }
 
   if (_return_mode.length() > 0)
-    new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode);
+    new_cfg.return_mode = nebula::drivers::ReturnModeFromStringSeyond(_return_mode,
+                                                                      sensor_cfg_ptr_->sensor_model);
 
   // ////////////////////////////////////////
   // Validate and use new config