From d10b249f8a1d19f727f83cf6887a98db6aae9de6 Mon Sep 17 00:00:00 2001 From: n-hamaike-esol <50005860+n-hamaike-esol@users.noreply.github.com> Date: Thu, 29 Jul 2021 20:46:46 +0900 Subject: [PATCH] fix for the issue with the termination process when powering off (#5) * add colcon build env. (base code is 0624e) * move some hpp files, add src folder in livox driver * trivial modification * Update README.md * remove unneccesary files * remove unneccesary header * move third-party source files * update code(20210630) * Delete different test files. * README.md add "4. running" * README crlf * HwInterface/udp_socket.hpp * modify for ros2 run error * Update README.md * accumulate_count * time_stamp * struct LivoxPointXyzrtl packed * GetEchoNumPerPoint() * cloud.width 24000 * time_base -> time_stamp * code fix * cloud.width update * code fix * code fix * StreamStart retry 30sec * code fix * rviz2 * fix format *
* format ros2wrapper * frame_id * frame_id * Update README.md * Update README.md * discussion_r664993039 * typo typo brief and only English in the comments. and LivoxDataCallback delete * PI -> M_PI * Revert "PI -> M_PI" This reverts commit c54bbe3b3c1954f82247d38570bdf71a3ddf38b6. * PI -> M_PI 2nd * thread pointer -> shared_ptr * modify CMakelists.txt and package.xml to using ament_cmake_auto * README.md * default node name * add composable node (#4) * add composable node * fix conflicts * fix exec node * change sensor name fix to variable * fix can use ros2_wrapper_main.cpp * README.md * default node name * change namespace * add namespace to topic names Co-authored-by: mitui.hide * package.xml maintainer and README.md * #discussion_r666683800 * #discussion_r666712110 * #discussion_r665125337 * #discussion_r664995104 * #discussion_r666078326 * #discussion_r665121366 * #discussion_r665121366 Expansion * typedef enum -> enum * typedef enum -> enum * #discussion_r665121708 * #discussion_r665124279 * #discussion_r664995791 * esol_ReviewNo6 * esol_ReviewNo26 * esol_ReviewNo24 * Revert "esol_ReviewNo24" This reverts commit 0e8f33341186d6fa4a905de45c8f9aa28b8d29cb. * Revert "esol_ReviewNo24" This reverts commit 0e8f33341186d6fa4a905de45c8f9aa28b8d29cb. * esol_ReviewNo24 * esol_ReviewNo27 * esol_ReviewNo17 * format * esol_ReviewNo21 * develop #1 6bae863876bfda52db848ed31e415a3b16a0b6a8 * tab -> space * develop marge * CMakeLists.txt remove(-g -O0) * T4PUB-358 1st * format and bug-fix * 5#discussion_r677475332 * Add comment semaphore_ * 5#discussion_r678802453 Co-authored-by: mitui.hide Co-authored-by: v-nagai7872@esol.co.jp Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: h-mitsui-esol <57085544+h-mitsui-esol@users.noreply.github.com> --- .../include/HwInterface/udp_socket.hpp | 2 +- hw_interface/src/udp_socket.cpp | 2 +- .../include/LidarDriver/lidar_driver.hpp | 105 ++++- livox_driver/src/lidar_driver.cpp | 445 ++++++++++-------- ros_wrapper/src/check_config.cpp | 2 +- ros_wrapper/src/livox_config.cpp | 35 +- ros_wrapper/src/ros2_driver_wrapper.cpp | 64 ++- ros_wrapper/src/ros2_driver_wrapper.hpp | 18 +- 8 files changed, 394 insertions(+), 279 deletions(-) diff --git a/hw_interface/include/HwInterface/udp_socket.hpp b/hw_interface/include/HwInterface/udp_socket.hpp index bc5c3a75e..1ad7bceee 100644 --- a/hw_interface/include/HwInterface/udp_socket.hpp +++ b/hw_interface/include/HwInterface/udp_socket.hpp @@ -22,7 +22,7 @@ class UdpSocket bool IsOpen() const; - int Send(std::vector & buff); + int Send(const std::vector & buff); int Recv(std::vector & buff, int wait_msec = -1); std::string EndPointIp() const; diff --git a/hw_interface/src/udp_socket.cpp b/hw_interface/src/udp_socket.cpp index b07e283b3..afe2d2bf1 100644 --- a/hw_interface/src/udp_socket.cpp +++ b/hw_interface/src/udp_socket.cpp @@ -125,7 +125,7 @@ uint16_t UdpSocket::EndPointPort() const { return ntohs(rcv_addrin_.sin_port); } /// @return On success, these calls return the number of bytes sent. /// @retval -1: sendto error /// @retval -2: port number not set. -int UdpSocket::Send(std::vector & buff) +int UdpSocket::Send(const std::vector & buff) { int retval; diff --git a/livox_driver/include/LidarDriver/lidar_driver.hpp b/livox_driver/include/LidarDriver/lidar_driver.hpp index 4f4c15ca9..0b46fbf37 100644 --- a/livox_driver/include/LidarDriver/lidar_driver.hpp +++ b/livox_driver/include/LidarDriver/lidar_driver.hpp @@ -1,7 +1,10 @@ #ifndef LIDARDRIVER_LIVOX_LIDARDRIVER_HPP_ #define LIDARDRIVER_LIVOX_LIDARDRIVER_HPP_ + +#include #include +#include #include #include "HwInterface/udp_socket.hpp" @@ -11,9 +14,6 @@ namespace lidar_driver { const uint16_t kCommandPort = 65000; -const uint8_t kCommandRxCommunicationSettingAck = 0x01; -const uint8_t kCommancRxStreamingAck = 0x04; -const uint8_t kCommandRxHeartbeatAck = 0x03; const uint16_t kCrcSeed16 = 0x4c49; const uint32_t kCrcSeed32 = 0x564f580a; const uint32_t kSdkPacketCrcSize = 4; @@ -38,6 +38,27 @@ enum class LidarCommandType { kLidarCommandHeartbeat, }; +enum class CommandResult { kAck = 0, kNack, kTimeout, kError, kUnknownPacket }; + +/** Enum that represents the command id. */ +enum class GeneralCommandID : uint8_t { + kBroadcast = 0, // General command set, broadcast command. + kHandshake = 1, // General command set, query the information of device. + kDeviceInfo = 2, // General command set, query the information of device. + kHeartbeat = 3, // General command set, heartbeat command. + kControlSample = 4, // General command set, enable or disable the sampling. + kCoordinateSystem = 5, // General command set, change the coordinate of point cloud data. + kDisconnect = 6, // General command set, disconnect the device. + kPushAbnormalState = 7, // General command set, a message command from a connected device + kConfigureStaticDynamicIp = 8, // General command set, set the IP of the a device. + kGetDeviceIpInformation = 9, // General command set, get the IP of the a device. + kRebootDevice = 0x0a, // General command set, reboot a device. + kSetDeviceParam = 0x0b, // Set device's parameters. + kGetDeviceParam = 0x0c, // Get device's parameters. + kResetDeviceParam = 0x0d, // Reset device's all parameters. + kCommandCount // Don't add command id after kCommandCount. +}; + #pragma pack(1) struct CommandHeader { @@ -94,11 +115,12 @@ struct LivoxPoint struct CommandHandshake { uint8_t cmd_set; - uint8_t cmd_id; + GeneralCommandID cmd_id; uint32_t host_ip; uint16_t data_port; uint16_t command_port; uint16_t imu_port; + uint32_t crc32; }; #pragma pack() @@ -106,7 +128,8 @@ struct CommandHandshake struct CommandHeartbeat { uint8_t cmd_set; - uint8_t cmd_id; + GeneralCommandID cmd_id; + uint32_t crc32; }; #pragma pack() @@ -114,23 +137,65 @@ struct CommandHeartbeat struct CommandSampling { uint8_t cmd_set; - uint8_t cmd_id; + GeneralCommandID cmd_id; uint8_t sample_ctrl; + uint32_t crc32; +}; +#pragma pack() + +#pragma pack(1) +struct CommandAll +{ + CommandHeader header; + union { + CommandHandshake hand_shake; + CommandHeartbeat heart_beat; + CommandSampling sampling; + } data; }; #pragma pack() #pragma pack(1) struct CommandAck { + CommandHeader header; uint8_t cmd_set; - uint8_t cmd_id; + GeneralCommandID cmd_id; uint8_t result; + uint32_t crc32; }; #pragma pack() -using EulerAngle = float [3]; /**< Roll, Pitch, Yaw, unit:radian. */ -using TranslationVector = float [3]; /**< x, y, z translation, unit: m. */ -using RotationMatrix = float [3][3]; +class Semaphore +{ +public: + explicit Semaphore(int count = 0) : count_(count) {} + + void Signal() + { + std::unique_lock lock(mutex_); + ++count_; + cv_.notify_one(); + } + + void Wait() + { + std::unique_lock lock(mutex_); + cv_.wait(lock, [=] { return count_ > 0; }); + --count_; + } + + int GetCount() { return count_; } + +private: + std::mutex mutex_; + std::condition_variable cv_; + volatile int count_; +}; + +using EulerAngle = float[3]; /**< Roll, Pitch, Yaw, unit:radian. */ +using TranslationVector = float[3]; /**< x, y, z translation, unit: m. */ +using RotationMatrix = float[3][3]; #pragma pack(1) struct ExtrinsicParameter @@ -186,10 +251,10 @@ class LidarDriver bool SetCalibration(); bool GetCalibration(); - int StartHwRxInterface(); + bool StartHwRxInterface(); void StopHwRxInterface(); - int StartHwTxInterface(); + CommandResult StartHwTxInterface(); void StopHwTxInterface(); int ParsePacket(livox_driver::LivoxLidarPacket & packet); @@ -217,10 +282,10 @@ class LidarDriver bool LivoxHwTxInterfaceHeartbeat(); bool SendCommand(LidarCommandType command); - void MakeCommandHandshake(std::vector & buff); - void MakeCommandStartStream(std::vector & buff); - void MakeCommandStopStream(std::vector & buff); - void MakeCommandHeartbeat(std::vector & buff); + CommandHandshake MakeCommandHandshake(); + CommandSampling MakeCommandStartStream(); + CommandSampling MakeCommandStopStream(); + CommandHeartbeat MakeCommandHeartbeat(); bool UdpSocketOpen( HwInterface::UdpSocket & sock, const std::string & sensor_ip, uint16_t sensor_port, @@ -228,6 +293,12 @@ class LidarDriver void CloseAllUdpSocket(); bool CheckRecvData(const std::vector & buff); + Semaphore semaphore_; + std::mutex mtx_; + std::vector MakeSendCommand(LidarCommandType command); + CommandResult SendAckWait(const std::vector & snd_buff, GeneralCommandID cmd_id); + GeneralCommandID GetCommandId(LidarCommandType cmd_type); + private: enum DeviceType { kHub = 0, /**< Livox Hub. */ @@ -302,7 +373,7 @@ class LidarDriver dst_point.x = raw_point.x / 1000.0f; dst_point.y = raw_point.y / 1000.0f; dst_point.z = raw_point.z / 1000.0f; - dst_point.reflectivity = (float)raw_point.reflectivity; + dst_point.reflectivity = static_cast(raw_point.reflectivity); } inline bool IsTripleFloatNoneZero(float x, float y, float z) { diff --git a/livox_driver/src/lidar_driver.cpp b/livox_driver/src/lidar_driver.cpp index 5099b17f0..193ddf5d5 100644 --- a/livox_driver/src/lidar_driver.cpp +++ b/livox_driver/src/lidar_driver.cpp @@ -42,15 +42,16 @@ bool LidarDriver::SetConfiguration(const livox_driver::LivoxSensorConfiguration is_success = UdpSocketOpen(rx_imusock_, param.sensor_ip, 0, param.imu_port); } - if (is_success != false) { - const int64_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ - buffer_time_ms_ = param.frequency_ms; - publish_period_ns_ = kNsPerSecond / (1000 / param.frequency_ms); - lidarconfig_ = param; - } else { + if (is_success == false) { CloseAllUdpSocket(); + return false; } + const int64_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ + buffer_time_ms_ = param.frequency_ms; + publish_period_ns_ = kNsPerSecond / (1000 / param.frequency_ms); + lidarconfig_ = param; + // set extrinsic parameter / (beta) -start float roll = -0.001; float pitch = 0.015; @@ -78,74 +79,124 @@ bool LidarDriver::GetConfiguration(livox_driver::LivoxSensorConfiguration & para return true; } -/// @brief send command to livox +/// @brief set command data /// @param command : command type -bool LidarDriver::SendCommand(LidarCommandType command) +std::vector LidarDriver::MakeSendCommand(LidarCommandType command) { - std::vector buff; - std::vector commanddata; - - CommandHeader headerdata; + std::vector buff(kMaxBufferSize); + CommandAll * command_all = reinterpret_cast(buff.data()); - uint8_t * crcbuf; - uint32_t crc16 = 0; - uint32_t crc32 = 0; - uint16_t headersize = sizeof(headerdata); - - headerdata.sof = 0xAA; - headerdata.version = 0x01; - headerdata.cmd_type = 0x00; - headerdata.seq_num = 0x00; + command_all->header.sof = 0xAA; + command_all->header.version = 0x01; + command_all->header.cmd_type = 0x00; + command_all->header.seq_num = 0x00; + uint32_t * crc32_ptr; + uint16_t cmd_size; switch (command) { case LidarCommandType::kLidarCommandHandshake: - MakeCommandHandshake(commanddata); + command_all->data.hand_shake = MakeCommandHandshake(); + crc32_ptr = &command_all->data.hand_shake.crc32; + cmd_size = sizeof(command_all->data.hand_shake); break; case LidarCommandType::kLidarCommandStartStreaming: - MakeCommandStartStream(commanddata); + command_all->data.sampling = MakeCommandStartStream(); + crc32_ptr = &command_all->data.sampling.crc32; + cmd_size = sizeof(command_all->data.sampling); break; case LidarCommandType::kLidarCommandStopStreaming: - MakeCommandStopStream(commanddata); + command_all->data.sampling = MakeCommandStopStream(); + crc32_ptr = &command_all->data.sampling.crc32; + cmd_size = sizeof(command_all->data.sampling); break; case LidarCommandType::kLidarCommandHeartbeat: - MakeCommandHeartbeat(commanddata); + command_all->data.heart_beat = MakeCommandHeartbeat(); + crc32_ptr = &command_all->data.heart_beat.crc32; + cmd_size = sizeof(command_all->data.heart_beat); break; } - uint16_t datasize = headersize + commanddata.size() + sizeof(crc32); - headerdata.length = datasize; - - crcbuf = reinterpret_cast(&headerdata); - - crc16 = crc16_.mcrf4xx_calc(crcbuf, headersize - kSdkPacketPreambleCrcSize); - headerdata.crc16 = crc16; + uint16_t header_size = sizeof(command_all->header); + uint16_t data_size = header_size + cmd_size; + command_all->header.length = data_size; - buff.resize(datasize, 0); - memcpy(&buff[0], &headerdata, headersize); - memcpy(&buff[headersize], &commanddata[0], commanddata.size()); + uint8_t * crcbuf = reinterpret_cast(&command_all->header); + uint32_t crc16 = crc16_.mcrf4xx_calc(crcbuf, header_size - kSdkPacketPreambleCrcSize); + command_all->header.crc16 = crc16; + *crc32_ptr = crc32_.crc32_calc(&buff[0], data_size - sizeof(*crc32_ptr)); + buff.resize(data_size); - crc32 = crc32_.crc32_calc(&buff[0], datasize - kSdkPacketCrcSize); - memcpy(&buff[datasize - kSdkPacketCrcSize], &crc32, sizeof(crc32)); + return buff; +} - int retval = commandsock_.Send(buff); +/// @brief Command send and ack wait. +/// @return CommandResult +/// @details Received unknown packet 10 retries. +CommandResult LidarDriver::SendAckWait( + const std::vector & snd_buff, GeneralCommandID cmd_id) +{ + //printf("%s Start cmd_id=%d\n", __func__, (int)cmd_id); + std::unique_lock lock(mtx_); + CommandResult result = CommandResult::kUnknownPacket; + int retval = commandsock_.Send(snd_buff); if (retval < 0) { - printf("command send error val:%d :%s(%d)\n", retval, __FILE__, __LINE__); + result = CommandResult::kError; // send error. + } else { + std::vector rcv_buff(kMaxBufferSize); + CommandAck * ack = reinterpret_cast(rcv_buff.data()); + + int rcv_len; + for (int retry_cnt = 10; retry_cnt > 0; --retry_cnt) { + rcv_len = commandsock_.Recv(rcv_buff, 100); // 100ms + if (rcv_len < 0) { + result = CommandResult::kError; // receive error + break; + } else if (rcv_len == 0) { + result = CommandResult::kTimeout; // timeout + break; + } else if (rcv_len < static_cast(sizeof(*ack))) { + } else if (CheckRecvData(rcv_buff) == false) { + } else if (ack->cmd_id != cmd_id) { + } else if (ack->result > 1) { + } else { + // ack->result 0:ack, 1:nack + result = (ack->result == 0) ? CommandResult::kAck : CommandResult::kNack; + break; + } + } + //printf("rcv_len=%d ack->result=%d\n", rcv_len, ack->result); } + //printf("%s End cmd_id=%d\n", __func__, (int)cmd_id); - return true; + return result; } -/// @brief Rx socket open and start receive thread -/// @retval 0 : success -/// @retval -1 : socket not open -/// @retval -2 : stream start timeout -/// @retval -3 : stream start ack result NG -int LidarDriver::StartHwRxInterface() +GeneralCommandID LidarDriver::GetCommandId(LidarCommandType cmd_type) { - int ret; + GeneralCommandID cmd_id; + switch (cmd_type) { + case LidarCommandType::kLidarCommandHandshake: + cmd_id = GeneralCommandID::kHandshake; + break; + case LidarCommandType::kLidarCommandStartStreaming: + cmd_id = GeneralCommandID::kControlSample; + break; + case LidarCommandType::kLidarCommandStopStreaming: + cmd_id = GeneralCommandID::kControlSample; + break; + case LidarCommandType::kLidarCommandHeartbeat: + cmd_id = GeneralCommandID::kHeartbeat; + break; + } + return cmd_id; +} +/// @brief Rx socket open and start receive thread +/// @return true:success, false:faile. +bool LidarDriver::StartHwRxInterface() +{ if (rx_datasock_.IsOpen() == false || rx_imusock_.IsOpen() == false) { - return -1; + return false; } if (thread_rxdata_.get() == nullptr) { @@ -154,156 +205,170 @@ int LidarDriver::StartHwRxInterface() if (thread_rximu_.get() == nullptr) { thread_rximu_ = std::make_shared(&LidarDriver::LivoxHwRxInterfaceImu, this); } + if (thread_rxcommand_.get() == nullptr) { + thread_rxcommand_ = + std::make_shared(&LidarDriver::LivoxHwRxInterfaceCommand, this); + } - // send start streaming - lidarstatus_ = LidarDriverStatus::kStreaming; - for (int cnt = 0; cnt < 30; cnt++) { - stream_start_ack_ = -2; - SendCommand(LidarCommandType::kLidarCommandStartStreaming); - for (int wait_50ms = 0; wait_50ms < 20; wait_50ms++) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - if (stream_start_ack_ == 0) { - return 0; - } - } // for(wait_50ms) - - if (lidarstatus_ != LidarDriverStatus::kStreaming) { - break; - } - } // for(cnt) - - ret = stream_start_ack_; - if (ret > 0) { - ret = -3; + // Send StartStreaming: wake up thread LivoxHwRxInterfaceCommand + if (semaphore_.GetCount() <= 0) { + semaphore_.Signal(); } - return ret; + return true; } /// @brief Rx socket close and stop receive thread void LidarDriver::StopHwRxInterface() { + if (lidarstatus_ == LidarDriverStatus::kStreaming) { + std::vector buff = MakeSendCommand(LidarCommandType::kLidarCommandStopStreaming); + GeneralCommandID cmd_id = GetCommandId(LidarCommandType::kLidarCommandStopStreaming); + CommandResult result = CommandResult::kTimeout; + + lidarstatus_ = LidarDriverStatus::kConnect; + for (int snd_cnt = 3; snd_cnt > 0; --snd_cnt) { + if (result != CommandResult::kTimeout) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + result = SendAckWait(buff, cmd_id); + if (result == CommandResult::kAck) { + break; + } + } + } + + // Stop thread LivoxHwRxInterfaceCommand + driverstatus_ = DriverStatus::kTerminate; + semaphore_.Signal(); + rx_datasock_.Close(); rx_imusock_.Close(); + + thread_rxcommand_->join(); thread_rxdata_->join(); thread_rximu_->join(); } /// @brief Tx socket open and start send thread -/// @retval 0 : success -/// @retval -1 : socket not open -/// @retval -2 : handshake timeout -/// @retval -3 : handshake ack result NG -int LidarDriver::StartHwTxInterface() +/// @return CommandResult +CommandResult LidarDriver::StartHwTxInterface() { - int iret = -3; - - if (commandsock_.IsOpen()) { - // send Handshake command - SendCommand(LidarCommandType::kLidarCommandHandshake); - std::vector buff; - buff.resize(kMaxBufferSize); - - // receive response - int rcv_len = commandsock_.Recv(buff, 1000); - if (rcv_len > 0) { - if (CheckRecvData(buff)) { - // check HandShake ack - CommandAck * ack = reinterpret_cast(&buff[sizeof(CommandHeader)]); - if (ack->cmd_id == kCommandRxCommunicationSettingAck && ack->result == 0x00) { - lidarstatus_ = LidarDriverStatus::kConnect; - // thread start - thread_txheartbeat_ = - std::make_shared(&LidarDriver::LivoxHwTxInterfaceHeartbeat, this); - thread_rxcommand_ = - std::make_shared(&LidarDriver::LivoxHwRxInterfaceCommand, this); - - iret = 0; - } - } - } else if (rcv_len == 0) { - // timeout - iret = -2; - } + CommandResult ret; + + if (commandsock_.IsOpen() == false) { + ret = CommandResult::kError; } else { - // socket not open - iret = -1; + std::vector buff = MakeSendCommand(LidarCommandType::kLidarCommandHandshake); + GeneralCommandID cmd_id = GetCommandId(LidarCommandType::kLidarCommandHandshake); + + ret = SendAckWait(buff, cmd_id); + if (ret == CommandResult::kAck) { + lidarstatus_ = LidarDriverStatus::kConnect; + // thread start + thread_txheartbeat_ = + std::make_shared(&LidarDriver::LivoxHwTxInterfaceHeartbeat, this); + } } - return iret; + + return ret; } /// @brief Tx socket close and stop send thread void LidarDriver::StopHwTxInterface() { - SendCommand(LidarCommandType::kLidarCommandStopStreaming); - driverstatus_ = DriverStatus::kTerminate; - thread_txheartbeat_->join(); - thread_rxcommand_->join(); - commandsock_.Close(); } /// @brief send thread bool LidarDriver::LivoxHwTxInterfaceHeartbeat() { - // wait 1.0 sec - std::this_thread::sleep_for(std::chrono::seconds(1)); - while (DriverStatus::kRunning == driverstatus_) { - if (LidarDriverStatus::kDisconnect == lidarstatus_) { - // send Handshake command - SendCommand(LidarCommandType::kLidarCommandHandshake); + std::vector hand_shake_buff = MakeSendCommand(LidarCommandType::kLidarCommandHandshake); + GeneralCommandID hand_shake_id = GetCommandId(LidarCommandType::kLidarCommandHandshake); + std::vector heart_beat_buff = MakeSendCommand(LidarCommandType::kLidarCommandHeartbeat); + GeneralCommandID heart_beat_id = GetCommandId(LidarCommandType::kLidarCommandHeartbeat); + CommandResult retval; + int retry_cnt = 0; + int wait_ms = 1000; // wait 1.0 sec + + while (driverstatus_ == DriverStatus::kRunning) { + std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms)); + if (driverstatus_ != DriverStatus::kRunning) { + //printf("%s Exit\n", __func__); + break; + } + wait_ms = 1000; + + if (lidarstatus_ == LidarDriverStatus::kDisconnect) { + retval = SendAckWait(hand_shake_buff, hand_shake_id); + if (retval == CommandResult::kAck) { + lidarstatus_ = LidarDriverStatus::kConnect; + retry_cnt = 0; + // Send StartStreaming: wake up thread LivoxHwRxInterfaceCommand + if (semaphore_.GetCount() <= 0) { + semaphore_.Signal(); + } + } else if (retval == CommandResult::kTimeout) { + wait_ms = 900; // 100 + 900 == 1000 + } else { + /* do nothing. */ + } } else { - // send heartbeat command - SendCommand(LidarCommandType::kLidarCommandHeartbeat); + retval = SendAckWait(heart_beat_buff, heart_beat_id); + if (retval == CommandResult::kAck) { + retry_cnt = 0; + } else if (++retry_cnt >= 3) { + lidarstatus_ = LidarDriverStatus::kDisconnect; + retry_cnt = 0; + wait_ms = 110; // requires 100ms over. + } else if (retval == CommandResult::kTimeout) { + wait_ms = 900; // 100 + 900 == 1000 + } else { + /* do nothing. */ + } } - // wait 1.0 sec - std::this_thread::sleep_for(std::chrono::seconds(1)); - } + } // while + return true; } /// @brief command receive thread bool LidarDriver::LivoxHwRxInterfaceCommand() { - std::vector buff; - buff.resize(kMaxBufferSize); - - while (DriverStatus::kRunning == driverstatus_) { - int rcv_len = commandsock_.Recv(buff); - - if (rcv_len > 0 && CheckRecvData(buff)) { - CommandAck * ack = reinterpret_cast(&buff[sizeof(CommandHeader)]); - switch (ack->cmd_id) { - case kCommandRxCommunicationSettingAck: - // first connection Communication port setting - DataRecvInit(); - break; - case kCommancRxStreamingAck: - stream_start_ack_ = ack->result; // 0:success, 1:fail. - //if (LidarDriverStatus::kConnect == lidarstatus_) { - // if (ack->result == 1) { - // // send retry(tentative) - // std::this_thread::sleep_for(std::chrono::milliseconds(100)); - // SendCommand(kLidarCommandStartStreaming); - // } else { - // // Start streaming - // lidarstatus_ = LidarDriverStatus::kStreaming; - // } - //} else { - // // Stop streaming - // //lidarstatus_ = LidarDriverStatus::kConnect; - //} - break; - case kCommandRxHeartbeatAck: - break; - default: - break; + std::vector snd_buff = MakeSendCommand(LidarCommandType::kLidarCommandStartStreaming); + GeneralCommandID cmd_id = GetCommandId(LidarCommandType::kLidarCommandStartStreaming); + + while (driverstatus_ == DriverStatus::kRunning) { + // Wait HandShake Ack(StartHwRxInterface or LivoxHwTxInterfaceHeartbeat) or StopHwRxInterface + semaphore_.Wait(); + + if (driverstatus_ != DriverStatus::kRunning) { + break; + } else if (lidarstatus_ == LidarDriverStatus::kDisconnect) { + // continue; + } else { + if (lidarstatus_ == LidarDriverStatus::kConnect) { + lidarstatus_ = LidarDriverStatus::kStreaming; } - } else if (rcv_len < 0) { - printf("socket receive error val:%d :%s(%d)\n", rcv_len, __FILE__, __LINE__); + + //printf("StartStreaming Loop start\n" ); + CommandResult result; + while (driverstatus_ == DriverStatus::kRunning && + lidarstatus_ == LidarDriverStatus::kStreaming) { + result = SendAckWait(snd_buff, cmd_id); + if (result == CommandResult::kAck) { + break; + } else if (result == CommandResult::kTimeout) { + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + } // while + //if (result != CommandResult::kAck) { printf("StartStreaming result=%d\n", (int)result ); } } } + return true; } @@ -335,49 +400,41 @@ void LidarDriver::CloseAllUdpSocket() } /// @brief make handshake command data -void LidarDriver::MakeCommandHandshake(std::vector & buff) +CommandHandshake LidarDriver::MakeCommandHandshake() { - buff.resize(sizeof(CommandHandshake), 0); - - CommandHandshake * command = reinterpret_cast(&buff[0]); - command->cmd_set = 0x00; - command->cmd_id = 0x01; - command->host_ip = inet_addr(lidarconfig_.host_ip.c_str()); - command->data_port = lidarconfig_.data_port; - command->command_port = lidarconfig_.cmd_port; - command->imu_port = lidarconfig_.imu_port; + CommandHandshake command{}; + command.cmd_id = GeneralCommandID::kHandshake; + command.host_ip = inet_addr(lidarconfig_.host_ip.c_str()); + command.data_port = lidarconfig_.data_port; + command.command_port = lidarconfig_.cmd_port; + command.imu_port = lidarconfig_.imu_port; + return command; } /// @brief make stream command data(streaming start) -void LidarDriver::MakeCommandStartStream(std::vector & buff) +CommandSampling LidarDriver::MakeCommandStartStream() { - buff.resize(sizeof(CommandSampling), 0); - - CommandSampling * command = reinterpret_cast(&buff[0]); - command->cmd_set = 0x00; - command->cmd_id = 0x04; - command->sample_ctrl = 0x01; + CommandSampling command{}; + command.cmd_id = GeneralCommandID::kControlSample; + command.sample_ctrl = 0x01; + return command; } /// @brief make stream command data(streaming stop) -void LidarDriver::MakeCommandStopStream(std::vector & buff) +CommandSampling LidarDriver::MakeCommandStopStream() { - buff.resize(sizeof(CommandSampling), 0); - - CommandSampling * command = reinterpret_cast(&buff[0]); - command->cmd_set = 0x00; - command->cmd_id = 0x04; - command->sample_ctrl = 0x00; + CommandSampling command{}; + command.cmd_id = GeneralCommandID::kControlSample; + command.sample_ctrl = 0x00; + return command; } /// @brief make heartbeat command data -void LidarDriver::MakeCommandHeartbeat(std::vector & buff) +CommandHeartbeat LidarDriver::MakeCommandHeartbeat() { - buff.resize(sizeof(CommandHeartbeat), 0); - - CommandHeartbeat * command = reinterpret_cast(&buff[0]); - command->cmd_set = 0x00; - command->cmd_id = 0x03; + CommandHeartbeat command{}; + command.cmd_id = GeneralCommandID::kHeartbeat; + return command; } /// @brief check receive data @@ -393,7 +450,7 @@ bool LidarDriver::CheckRecvData(const std::vector & buff) uint16_t crc16 = commandheader->crc16; // check crc16 on headder - crc16_calc = crc16_.mcrf4xx_calc(&buff[0], sizeof(CommandHeader) - kSdkPacketPreambleCrcSize); + crc16_calc = crc16_.mcrf4xx_calc(&buff[0], sizeof(*commandheader) - kSdkPacketPreambleCrcSize); if (crc16 != crc16_calc) { return false; } @@ -416,9 +473,9 @@ int LidarDriver::ParsePacket(livox_driver::LivoxLidarPacket & packet) bool timegap_over = false; LivoxEthPacket * raw_packet = reinterpret_cast(packet.data.data()); - if( temp_publish_data_.get() == nullptr ) { + if (temp_publish_data_.get() == nullptr) { temp_publish_data_ = std::make_unique(); - } + } if (temp_publish_data_->num == 0) { uint32_t remaining_time = timestamp % publish_period_ns_; @@ -504,7 +561,8 @@ uint8_t * LidarDriver::LivoxExtendRawPointToPxyzrtl( uint8_t * point_buf, LivoxEthPacket * eth_packet, ExtrinsicParameter & extrinsic, uint32_t line_num) { - livox_driver::LivoxPointXyzrtl * dst_point = (livox_driver::LivoxPointXyzrtl *)point_buf; + livox_driver::LivoxPointXyzrtl * dst_point = + reinterpret_cast(point_buf); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxExtendRawPoint * raw_point = reinterpret_cast(eth_packet->data); @@ -512,8 +570,9 @@ uint8_t * LidarDriver::LivoxExtendRawPointToPxyzrtl( while (points_per_packet) { RawPointConvert(*dst_point, reinterpret_cast(*raw_point)); if (extrinsic.enable && IsTripleIntNoneZero(raw_point->x, raw_point->y, raw_point->z)) { - livox_driver::PointXyz src_point = *((livox_driver::PointXyz *)dst_point); - PointExtrisincCompensation((livox_driver::PointXyz *)dst_point, src_point, extrinsic); + livox_driver::PointXyz * xyz_ptr = reinterpret_cast(dst_point); + livox_driver::PointXyz src_point = *xyz_ptr; + PointExtrisincCompensation(xyz_ptr, src_point, extrinsic); } dst_point->tag = raw_point->tag; if (line_num > 1) { diff --git a/ros_wrapper/src/check_config.cpp b/ros_wrapper/src/check_config.cpp index 09f3d4ca0..117584e74 100644 --- a/ros_wrapper/src/check_config.cpp +++ b/ros_wrapper/src/check_config.cpp @@ -32,7 +32,7 @@ void RosDriverWrapper::GetParameter() /// @brief check IP address. /// @param ip_addr /// @return true: good address. false: bad address. -bool RosDriverWrapper::CheckIpAddress(std::string & ip_addr) +bool RosDriverWrapper::CheckIpAddress(const std::string & ip_addr) { bool ret = false; if (!ip_addr.empty()) { diff --git a/ros_wrapper/src/livox_config.cpp b/ros_wrapper/src/livox_config.cpp index f817f46fa..4de219d8a 100644 --- a/ros_wrapper/src/livox_config.cpp +++ b/ros_wrapper/src/livox_config.cpp @@ -67,8 +67,8 @@ RosDriverWrapper::LivoxCoordinateModeEx RosDriverWrapper::GetParamLivoxCoordinat } /// @brief get livox configuration parameter. -void RosDriverWrapper::GetLivoxParameter(SensorConfigEx & ex, - livox_driver::LivoxCloudConfiguration & cloud_config) +void RosDriverWrapper::GetLivoxParameter( + SensorConfigEx & ex, livox_driver::LivoxCloudConfiguration & cloud_config) { std::string str_coordinate_mode; @@ -90,7 +90,7 @@ void RosDriverWrapper::GetLivoxParameter(SensorConfigEx & ex, LivoxSensorModelEx sensor_model_ex = GetParamLivoxSensorModel(); ex.sensor_config.sensor_model = std::get<0>(sensor_model_ex); ex.sensor_model_str = std::get<1>(sensor_model_ex); - + // LivoxSensorConfiguration this->get_parameter("host_ip", ex.sensor_config.host_ip); this->get_parameter("sensor_ip", ex.sensor_config.sensor_ip); @@ -128,33 +128,27 @@ bool RosDriverWrapper::CheckLivoxSensorConfiguration(SensorConfigEx & ex) if (ex.sensor_config.sensor_model == livox_driver::LivoxSensorModel::UNKNOWN) { err_str = "sensor_model '" + ex.sensor_model_str + "'"; - } - else if (CheckIpAddress(ex.sensor_config.host_ip) == false) { + } else if (CheckIpAddress(ex.sensor_config.host_ip) == false) { err_str = "host_ip '" + ex.sensor_config.host_ip + "'"; - } - else if (CheckIpAddress(ex.sensor_config.sensor_ip) == false) { + } else if (CheckIpAddress(ex.sensor_config.sensor_ip) == false) { err_str = "sensor_ip '" + ex.sensor_config.sensor_ip + "'"; - } - else if (CheckPortNumber(ex.sensor_config.data_port) == false) { + } else if (CheckPortNumber(ex.sensor_config.data_port) == false) { err_str = "data_port " + std::to_string(ex.sensor_config.data_port); - } - else if (CheckPortNumber(ex.sensor_config.imu_port) == false) { + } else if (CheckPortNumber(ex.sensor_config.imu_port) == false) { err_str = "imu_port " + std::to_string(ex.sensor_config.imu_port); - } - else if (CheckPortNumber(ex.sensor_config.cmd_port) == false) { + } else if (CheckPortNumber(ex.sensor_config.cmd_port) == false) { err_str = "cmd_port " + std::to_string(ex.sensor_config.cmd_port); - } - else if ((ex.sensor_config.frequency_ms < 5) || (ex.sensor_config.frequency_ms > 1000)) { + } else if ((ex.sensor_config.frequency_ms < 5) || (ex.sensor_config.frequency_ms > 1000)) { err_str = "frequency_ms " + std::to_string(ex.sensor_config.frequency_ms); } -#if 0 // ToDo: echo_mode and coordinate_mode must not be implemented in the first beta version of the driver. +#if 0 // ToDo: echo_mode and coordinate_mode must not be implemented in the first beta version of the driver. else if( ex.sensor_config.echo_mode == livox_driver::LivoxEchoMode::UNKNOWN ) { err_str = "echo_mode '" + ex.sensor_config.echo_mode_str + "'"; } else if( ex.sensor_config.coordinate_model == livox_driver::LivoxCoordinateMode::UNKNOWN ) { err_str = "coordinate_mode '" + ex.sensor_config.coordinate_mode_str + "'"; } -#endif // ToDo: echo_mode and coordinate_mode must not be implemented in the first beta version of the driver. +#endif // ToDo: echo_mode and coordinate_mode must not be implemented in the first beta version of the driver. else { ret = true; } @@ -172,8 +166,8 @@ bool RosDriverWrapper::CheckLivoxSensorConfiguration(SensorConfigEx & ex) /// @param cloud_config: Livox Cloud Configuration /// @return true on success, false on failure. /// @details boolean is No check required. -bool RosDriverWrapper::CheckLivoxCloudConfiguration(SensorConfigEx & ex, - livox_driver::LivoxCloudConfiguration & cloud_config) +bool RosDriverWrapper::CheckLivoxCloudConfiguration( + SensorConfigEx & ex, livox_driver::LivoxCloudConfiguration & cloud_config) { std::string err_str = ""; bool ret = false; @@ -193,8 +187,7 @@ bool RosDriverWrapper::CheckLivoxCloudConfiguration(SensorConfigEx & ex, err_str = "cloud_min_range" + std::to_string(cloud_config.cloud_min_range); } else if (std::signbit(cloud_config.cloud_max_range)) { err_str = "cloud_max_range" + std::to_string(cloud_config.cloud_max_range); - } - else { + } else { ret = true; } diff --git a/ros_wrapper/src/ros2_driver_wrapper.cpp b/ros_wrapper/src/ros2_driver_wrapper.cpp index 03d2aae1f..439951480 100644 --- a/ros_wrapper/src/ros2_driver_wrapper.cpp +++ b/ros_wrapper/src/ros2_driver_wrapper.cpp @@ -46,45 +46,42 @@ RosDriverWrapper::~RosDriverWrapper() } /// @brief Shutdown -void RosDriverWrapper::Shutdown() -{ - StreamStop(); - exit_signal_.set_value(); -} +void RosDriverWrapper::Shutdown() { exit_signal_.set_value(); } /// @brief StreamStop void RosDriverWrapper::StreamStop() { - driver_.StopHwTxInterface(); - driver_.StopHwRxInterface(); + RCLCPP_INFO(this->get_logger(), "StopHwRxInterface: StreamStop request."); + driver_.StopHwRxInterface(); // stream stop request. + driver_.StopHwTxInterface(); // stop heart beat. } /// @brief StartHwTxInterface bool RosDriverWrapper::StartInterface() { bool ret = false; - int pre_rc = 0; - int rc; + CommandResult pre_result = CommandResult::kAck; + CommandResult cmd_result; int wait_ms = 0; - std::future_status status; + std::future_status status = std::future_status::timeout; do { - rc = driver_.StartHwTxInterface(); - if (rc == 0) { + cmd_result = driver_.StartHwTxInterface(); + if (cmd_result == CommandResult::kAck) { ret = true; RCLCPP_INFO(this->get_logger(), "StartHwTxInterface: Handshake success."); break; - } else if (rc == -2) { + } else if (cmd_result == CommandResult::kTimeout) { // Timeout. - if (pre_rc != rc) { - pre_rc = rc; + if (pre_result != cmd_result) { + pre_result = cmd_result; RCLCPP_WARN(this->get_logger(), "StartHwTxInterface: Handshake Timeout"); } - wait_ms = 0; - } else if (rc == -3) { + wait_ms = 400; + } else if (cmd_result == CommandResult::kNack) { // Ack fail. - if (pre_rc != rc) { - pre_rc = rc; + if (pre_result != cmd_result) { + pre_result = cmd_result; RCLCPP_WARN(this->get_logger(), "StartHwTxInterface: Handshake Ack fail."); } wait_ms = 500; @@ -96,6 +93,11 @@ bool RosDriverWrapper::StartInterface() status = future_.wait_for(std::chrono::milliseconds(wait_ms)); } while (status == std::future_status::timeout); + if (status != std::future_status::timeout) { + RCLCPP_ERROR(this->get_logger(), "StartHwTxInterface Cancel %d", static_cast(cmd_result)); + ret = false; + } + return ret; } @@ -138,7 +140,6 @@ void RosDriverWrapper::CreatePubSub() /// @brief StreamStart bool RosDriverWrapper::StreamStart() { - bool ret = false; /// InstantiateLidarDriver /// Instantiates the corresponding Lidar Driver for the selected sensor. @@ -153,22 +154,11 @@ bool RosDriverWrapper::StreamStart() driver_.GetSensorConfig( sensor_configuration_ ); #endif // TODO(Next phase) - // Figure 10. LidarDriver.StreamStart() - int rc = driver_.StartHwRxInterface(); - switch (rc) { - case 0: - RCLCPP_INFO(this->get_logger(), "StartHwRxInterface: StreamStart success."); - ret = true; - break; - case -1: - RCLCPP_ERROR(this->get_logger(), "StartHwRxInterface: Sensor Socket error !"); - break; - case -2: - RCLCPP_ERROR(this->get_logger(), "StartHwRxInterface: StreamStart Timeout."); - break; - case -3: - RCLCPP_ERROR(this->get_logger(), "StartHwRxInterface: StreamStart Ack fail."); - break; + bool ret = driver_.StartHwRxInterface(); + if (ret == false) { + RCLCPP_ERROR(this->get_logger(), "StartHwRxInterface: StreamStart failed."); + } else { + RCLCPP_INFO(this->get_logger(), "StartHwRxInterface: StreamStart request."); } return ret; @@ -208,6 +198,8 @@ void RosDriverWrapper::MainThread() status = future_.wait_for(std::chrono::seconds(3)); } while (status == std::future_status::timeout); + StreamStop(); + RCLCPP_INFO(this->get_logger(), "LidarDriver Ros2Wrapper MainThread() end %d", (int)status); } diff --git a/ros_wrapper/src/ros2_driver_wrapper.hpp b/ros_wrapper/src/ros2_driver_wrapper.hpp index 923103727..ed9b4a8d6 100644 --- a/ros_wrapper/src/ros2_driver_wrapper.hpp +++ b/ros_wrapper/src/ros2_driver_wrapper.hpp @@ -31,7 +31,7 @@ class RosDriverWrapper final : public rclcpp::Node { public: // Figure 9. explicit RosDriverWrapper( - const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), const std::string & node_name = "lidar_driver_node"); ~RosDriverWrapper() override; @@ -88,10 +88,11 @@ class RosDriverWrapper final : public rclcpp::Node std::string debug_str2; int npackets; } config_; - + //SensorConfigEx sensor_configuration_; - struct SensorConfigEx_ { - SensorConfiguration sensor_config; + struct SensorConfigEx_ + { + SensorConfiguration sensor_config; std::string sensor_model_str; std::string echo_mode_str; std::string coordinate_mode_str; @@ -108,7 +109,7 @@ class RosDriverWrapper final : public rclcpp::Node SensorConfiguration GetSensorConfig(); void GetParameter(); - bool CheckIpAddress(std::string & ip_addr); + bool CheckIpAddress(const std::string & ip_addr); bool CheckPortNumber(int port) { return ((0 < port) && (port <= 0xFFFF)) ? true : false; } using LivoxSensorModelEx = std::tuple; @@ -118,11 +119,10 @@ class RosDriverWrapper final : public rclcpp::Node using LivoxCoordinateModeEx = std::tuple; LivoxCoordinateModeEx GetParamLivoxCoordinateMode(); - void GetLivoxParameter(SensorConfigEx & ex, - livox_driver::LivoxCloudConfiguration & cloud_config); + void GetLivoxParameter(SensorConfigEx & ex, livox_driver::LivoxCloudConfiguration & cloud_config); bool CheckLivoxSensorConfiguration(SensorConfigEx & ex); - bool CheckLivoxCloudConfiguration(SensorConfigEx & ex, - livox_driver::LivoxCloudConfiguration & cloud_config); + bool CheckLivoxCloudConfiguration( + SensorConfigEx & ex, livox_driver::LivoxCloudConfiguration & cloud_config); #ifdef UTEST public: