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 7b11b4a26..70e65f002 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 @@ -27,38 +27,49 @@ #include #include -#include #include 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 m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr sensor_configuration_; std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ - int prev_phase_{}; - int target_model_no; + std::shared_ptr m_owned_ctx_; + std::unique_ptr<::drivers::tcp_driver::TcpDriver> command_tcp_driver_; + std::shared_ptr 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 sensor_configuration); + const std::shared_ptr& 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 logger); + + /// @brief Display common information acquired from sensor + void DisplayCommonVersion(); }; } // 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 bd9737203..c1e84ad8d 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 @@ -1,21 +1,65 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp" + +#include + 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()), + 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 & 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,44 +69,71 @@ Status SeyondHwInterface::SensorInterfaceStop() } Status SeyondHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + const std::shared_ptr& 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(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 &)> 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(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 logger) @@ -70,5 +141,90 @@ void SeyondHwInterface::SetLogger(std::shared_ptr logger) parent_node_logger_ = logger; } +void SeyondHwInterface::DisplayCommonVersion() +{ + std::stringstream info; + info << "*************** Innovusion Lidar Version Info ***************" <GetIOContext()->stopped()) { + command_tcp_driver_->GetIOContext()->restart(); + } + + std::vector 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(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(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 diff --git a/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp index bf5a976f4..741326706 100755 --- a/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp +++ b/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 & config); + SeyondHwInterfaceWrapper() = default; void OnConfigChange(const std::shared_ptr & new_config); @@ -28,10 +29,10 @@ class SeyondHwInterfaceWrapper std::shared_ptr HwInterface() const; private: - std::shared_ptr hw_interface_; + std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; - nebula::Status status_; - bool setup_sensor_; + nebula::Status status_{}; + bool setup_sensor_{}; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp index b04e72083..ae19e153d 100644 --- a/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace nebula { @@ -57,19 +58,19 @@ class SeyondRosWrapper final : public rclcpp::Node Status ValidateAndSetConfig(std::shared_ptr & new_config); - Status wrapper_status_; + Status wrapper_status_{}; std::shared_ptr sensor_cfg_ptr_{}; - std::mutex mtx_config_; + std::mutex mtx_config_{}; mt_queue> packet_queue_; /// @brief Thread to isolate decoding from receiving - std::thread decoder_thread_; + std::thread decoder_thread_{}; rclcpp::Subscription::SharedPtr packets_sub_{}; - bool launch_hw_; + bool launch_hw_{}; std::optional hw_interface_wrapper_; std::optional hw_monitor_wrapper_; diff --git a/nebula_ros/src/seyond/hw_interface_wrapper.cpp b/nebula_ros/src/seyond/hw_interface_wrapper.cpp index 2223fcee0..f3fc7bc3b 100644 --- a/nebula_ros/src/seyond/hw_interface_wrapper.cpp +++ b/nebula_ros/src/seyond/hw_interface_wrapper.cpp @@ -7,14 +7,16 @@ namespace ros SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config) -: hw_interface_(new SeyondHwInterface()), +: hw_interface_(std::make_shared()), logger_(parent_node->get_logger().get_child("HwInterface")), - status_(Status::NOT_INITIALIZED) + status_(Status::NOT_INITIALIZED), + 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(config); + status_ = hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(config)); if (status_ != Status::OK) { throw std::runtime_error( @@ -46,7 +48,8 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( void SeyondHwInterfaceWrapper::OnConfigChange( const std::shared_ptr & new_config) { - hw_interface_->SetSensorConfiguration(new_config); + hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(new_config)); // if (setup_sensor_) { // hw_interface_->CheckAndSetConfig(); // }