From 0f081d2ab37bbb02ef747e0fb1802759f7e4c895 Mon Sep 17 00:00:00 2001 From: Manato HIRABAYASHI Date: Wed, 19 Jun 2024 19:25:55 +0900 Subject: [PATCH] feat: add support to set return mode Signed-off-by: Manato HIRABAYASHI --- .../nebula_common/seyond/seyond_common.hpp | 33 ++++++++++- .../seyond_hw_interface.hpp | 13 +++++ .../seyond_hw_interface.cpp | 55 +++++++++++++++---- .../src/seyond/hw_interface_wrapper.cpp | 36 +++++++----- nebula_ros/src/seyond/seyond_ros_wrapper.cpp | 5 +- 5 files changed, 111 insertions(+), 31 deletions(-) diff --git a/nebula_common/include/nebula_common/seyond/seyond_common.hpp b/nebula_common/include/nebula_common/seyond/seyond_common.hpp index 6d6ea3cba..9e9ac20ef 100755 --- a/nebula_common/include/nebula_common/seyond/seyond_common.hpp +++ b/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 \ No newline at end of file +#endif // NEBULA_SEYOND_COMMON_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp index 70e65f002..eb2da3190 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp +++ b/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 & 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 diff --git a/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp index c1e84ad8d..014d1792b 100644 --- a/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp @@ -20,20 +20,13 @@ SeyondHwInterface::~SeyondHwInterface() { } -void SeyondHwInterface::ReceiveSensorPacketCallback(std::vector & 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 & 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 diff --git a/nebula_ros/src/seyond/hw_interface_wrapper.cpp b/nebula_ros/src/seyond/hw_interface_wrapper.cpp index f3fc7bc3b..01e3411fb 100644 --- a/nebula_ros/src/seyond/hw_interface_wrapper.cpp +++ b/nebula_ros/src/seyond/hw_interface_wrapper.cpp @@ -13,7 +13,7 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( setup_sensor_(false) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); - //bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); status_ = hw_interface_->SetSensorConfiguration( std::static_pointer_cast(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(new_config)); - // if (setup_sensor_) { - // hw_interface_->CheckAndSetConfig(); - // } + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } } Status SeyondHwInterfaceWrapper::Status() diff --git a/nebula_ros/src/seyond/seyond_ros_wrapper.cpp b/nebula_ros/src/seyond/seyond_ros_wrapper.cpp index 0eae442fb..8563e8122 100644 --- a/nebula_ros/src/seyond/seyond_ros_wrapper.cpp +++ b/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("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("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