From f1bdaf4a350f79110b90eb3a7510b2af40bbfc1b Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 23 Aug 2024 16:38:50 +0900 Subject: [PATCH] =?UTF-8?q?2.1.0=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E3=81=9F=E3=82=81=E3=81=ABros2-devel=E3=81=AE?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=E5=86=85=E5=AE=B9=E3=82=92humble-devel?= =?UTF-8?q?=E3=81=B8=E3=83=9E=E3=83=BC=E3=82=B8=20(#60)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add unit tests (#52) * Use pointer for SerialPort instance instead of inheritance * Add test using fakeit * Install git comand in indsutrial_ci * Refactoring * Update CI * Use AFTER_SETUP_TARGET_WORKSPACE * driverインスタンスのメンバ変数のテストを追加 * バイナリデータが読み込まれたことを確認するテストを追加 * ASCIIデータが読み込まれたことを確認するテストを追加 * バイナリでもASCIIでもないデータが読み込まれたことを確認するテストを追加 * テストが通る状態に修正 --------- Co-authored-by: ShotaAk * checkDataFormat() and unit test updates (#54) * Binaryデータ出力時は実機とテストの両方で動作確認完了、ASCIIデータには未対応、デバッグ用出力あり * 実機とテストの両方でBinaryとASCIIの判定に成功、BinaryでもASCIIでもないデータには未対応 * タイムアウト機能を追加&hasCompletedFormatCheck()と関係する変数を削除 * 不正なデータをチェックするようにテストを修正 * readを成功させるためにwhileループにsleep処理を追加 * 必要がなかったため、while文のsleep処理を削除 * テスト用データの作成で重複している箇所を関数化 * RCLCPP_INFOをWARNに変更&不要なコメントを削除 * const autoをできる限り使用 * actions/checkoutのバージョンを3から4に更新 * 読み取ったデータをある程度貯めてからデータ形式を判定するように変更 * 貯めるデータを更新するように修正 * メンバ変数の名前を修正&256を定数化 * Add test for readSensorData() (#57) * readSensorData()実行後のhasRefreshedImuData()の応答のテストを追加 * テストにコメントを追加 * 0.0を入力として与えたテストを追加 * ASCII形式のテストデータを作成する際、引数で値を渡せるように変更 * Binary形式でもテストデータを引数で渡せるように変更 * テストケースを使い回せるように変更 * readSensorData()実行時にセンサデータが正しく変換されたか検証するテストを追加 * デバッグ用の関数を削除 * short intをint16_tに変更 * int16を8bitずつに分ける関数をhighとlowそれぞれ用意 * 既存のテストに影響を与えないようにデータ作成の関数をオーバーロード * set_data関数をprivateに変更 * 変換後の数値を直打ち * data_format_を設定する関数を削除 * 不要な箇所を削除 * Fix to get the latest data (#58) * 最新データ取得のテストを追加 * PR#50を参考に最新のデータを取得できるように変更 * 複数セットのデータ取得時に最新のデータをセットするように変更、テストは通るが挙動がおかしい * 最新データの取得方法を修正 * 取得するデータが適切な長さであることを保証する * 2.1.0リリースのためにCHANGELOG.rstとpackage.xmlを更新 (#59) * CHANGELOGを更新 * 2.1.0 --------- Co-authored-by: ShotaAk --- .github/workflows/industrial_ci.yml | 4 +- CHANGELOG.rst | 9 + CMakeLists.txt | 20 +- .../rt_usb_9axisimu.hpp | 16 +- .../rt_usb_9axisimu_driver.hpp | 70 +- package.xml | 2 +- src/rt_usb_9axisimu_driver.cpp | 197 ++++-- src/rt_usb_9axisimu_driver_component.cpp | 42 +- test/test_driver.cpp | 621 ++++++++++++++++++ 9 files changed, 866 insertions(+), 115 deletions(-) create mode 100644 test/test_driver.cpp diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 984d3d5..042d926 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -15,10 +15,10 @@ jobs: strategy: matrix: env: - - { ROS_DISTRO: humble, ROS_REPO: ros } + - { ROS_DISTRO: humble, ROS_REPO: ros, AFTER_SETUP_TARGET_WORKSPACE: 'apt update && apt install -y git' } runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: "ros-industrial/industrial_ci@master" env: ${{ matrix.env }} diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 1baabd6..2d3c3c9 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rt_usb_9axisimu_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.0 (2024-08-23) +------------------ +* Fix to get the latest data (`#58 `_) +* Add test for readSensorData() (`#57 `_) +* checkDataFormat() and unit test updates (`#54 `_) +* Add unit tests (`#52 `_) + Co-authored-by: ShotaAk +* Contributors: YusukeKato + 2.0.2 (2023-01-26) ------------------ * Support Humble (`#42 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2711b4c..2db4440 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,9 +82,25 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -#if(BUILD_TESTING) +if(BUILD_TESTING) # find_package(ament_lint_auto REQUIRED) # ament_lint_auto_find_test_dependencies() -#endif() + Set(FETCHCONTENT_QUIET FALSE) + include(FetchContent) + FetchContent_Declare( + fakeit + GIT_REPOSITORY https://github.com/eranpeer/FakeIt + GIT_TAG 2.4.0 + GIT_PROGRESS TRUE) + FetchContent_MakeAvailable(fakeit) + + find_package(ament_cmake_gtest) + ament_add_gtest(test_driver test/test_driver.cpp src/rt_usb_9axisimu_driver.cpp) + target_include_directories(test_driver PRIVATE ${fakeit_SOURCE_DIR}/single_header/gtest) + ament_target_dependencies(test_driver + rclcpp + sensor_msgs + ) +endif() ament_package() \ No newline at end of file diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp index 35970f1..f555307 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp @@ -106,6 +106,8 @@ class Consts IMU_ASCII_DATA_SIZE }; + static constexpr int READ_BUFFER_SIZE = 512; + // Convertor const double CONVERTOR_RAW2G; const double CONVERTOR_RAW2DPS; @@ -178,23 +180,23 @@ class SerialPort { } - ~SerialPort() + virtual ~SerialPort() { closeSerialPort(); } - void setPort(const char * port) + virtual void setPort(const char * port) { port_name_ = port; } - bool openPort(const char * port) + virtual bool openPort(const char * port) { port_name_ = port; return openSerialPort(); } - bool openSerialPort() + virtual bool openSerialPort() { int fd = 0; @@ -221,7 +223,7 @@ class SerialPort return fd > 0; } - void closeSerialPort() + virtual void closeSerialPort() { if (port_fd_ > 0) { tcsetattr(port_fd_, TCSANOW, &old_settings_); @@ -230,7 +232,7 @@ class SerialPort } } - int readFromDevice(unsigned char * buf, unsigned int buf_len) + virtual int readFromDevice(unsigned char * buf, unsigned int buf_len) { if (port_fd_ < 0) { return -1; @@ -239,7 +241,7 @@ class SerialPort return read(port_fd_, buf, buf_len); } - int writeToDevice(unsigned char * data, unsigned int data_len) + virtual int writeToDevice(unsigned char * data, unsigned int data_len) { if (port_fd_ < 0) { return -1; diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp index 18d62c1..05a20f8 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp @@ -44,14 +44,38 @@ #include "sensor_msgs/msg/magnetic_field.hpp" #include "std_msgs/msg/float64.hpp" -class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort +class RtUsb9axisimuRosDriver { -private: - // ros::NodeHandle nh_; +public: + explicit RtUsb9axisimuRosDriver(std::string serialport); + RtUsb9axisimuRosDriver(std::unique_ptr serial_port); + ~RtUsb9axisimuRosDriver(); - // ros::Publisher imu_data_raw_pub_; - // ros::Publisher imu_mag_pub_; - // ros::Publisher imu_temperature_pub_; + enum ReadStatus + { + SUCCESS = 0, + NEED_TO_CONTINUE, + FAILURE + }; + + void setImuFrameIdName(std::string frame_id); + void setImuPortName(std::string port); + void setImuStdDev(double linear_acceleration, double angular_velocity, double magnetic_field); + + bool startCommunication(); + void stopCommunication(void); + void checkDataFormat(const double timeout = 5.0); + bool hasAsciiDataFormat(void); + bool hasBinaryDataFormat(void); + bool hasRefreshedImuData(void); + + std::unique_ptr getImuRawDataUniquePtr(const rclcpp::Time timestamp); + std::unique_ptr getImuMagUniquePtr(const rclcpp::Time timestamp); + std::unique_ptr getImuTemperatureUniquePtr(void); + ReadStatus readSensorData(); + +private: + std::unique_ptr serial_port_; rt_usb_9axisimu::SensorData sensor_data_; @@ -61,6 +85,11 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort double magnetic_field_stddev_; rt_usb_9axisimu::Consts consts; + unsigned char bin_read_buffer_[rt_usb_9axisimu::Consts::READ_BUFFER_SIZE]; + unsigned char ascii_read_buffer_[rt_usb_9axisimu::Consts::READ_BUFFER_SIZE]; + unsigned int bin_read_buffer_idx_ = 0; + unsigned int ascii_read_buffer_idx_ = 0; + enum DataFormat { NONE = 0, @@ -70,7 +99,6 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort ASCII, INCORRECT }; - bool has_completed_format_check_; DataFormat data_format_; bool has_refreshed_imu_data_; @@ -78,31 +106,11 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort int16_t combineByteData(unsigned char data_h, unsigned char data_l); // Method to extract binary sensor data from communication buffer rt_usb_9axisimu::ImuData extractBinarySensorData(unsigned char * imu_data_buf); - bool isBinarySensorData(unsigned char * imu_data_buf); - bool readBinaryData(void); + bool isBinarySensorData(unsigned char * imu_data_buf, unsigned int data_size); + ReadStatus readBinaryData(void); + bool isAsciiSensorData(unsigned char * imu_data_buf, unsigned int data_size); bool isValidAsciiSensorData(std::vector imu_data_vector_buf); - bool readAsciiData(void); - -public: - explicit RtUsb9axisimuRosDriver(std::string serialport); - ~RtUsb9axisimuRosDriver(); - - void setImuFrameIdName(std::string frame_id); - void setImuPortName(std::string port); - void setImuStdDev(double linear_acceleration, double angular_velocity, double magnetic_field); - - bool startCommunication(); - void stopCommunication(void); - void checkDataFormat(void); - bool hasCompletedFormatCheck(void); - bool hasAsciiDataFormat(void); - bool hasBinaryDataFormat(void); - bool hasRefreshedImuData(void); - - std::unique_ptr getImuRawDataUniquePtr(const rclcpp::Time timestamp); - std::unique_ptr getImuMagUniquePtr(const rclcpp::Time timestamp); - std::unique_ptr getImuTemperatureUniquePtr(void); - bool readSensorData(); + ReadStatus readAsciiData(void); }; #endif // RT_USB_9AXISIMU_DRIVER__RT_USB_9AXISIMU_DRIVER_HPP_ diff --git a/package.xml b/package.xml index 6f61f2a..c7c4671 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ rt_usb_9axisimu_driver - 2.0.2 + 2.1.0 The rt_usb_9axisimu_driver package RT Corporation diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index 934e7a7..3bb4588 100644 --- a/src/rt_usb_9axisimu_driver.cpp +++ b/src/rt_usb_9axisimu_driver.cpp @@ -31,11 +31,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include -#include #include #include +#include #include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp" @@ -83,48 +82,73 @@ RtUsb9axisimuRosDriver::extractBinarySensorData(unsigned char * imu_data_buf) return imu_rawdata; } -bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf) +bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf, unsigned int data_size) { - if (imu_data_buf[consts.IMU_BIN_HEADER_R] == 'R' && - imu_data_buf[consts.IMU_BIN_HEADER_T] == 'T') + for (int i = 0; i < (int)data_size; i++) { + bin_read_buffer_[bin_read_buffer_idx_] = imu_data_buf[i]; + bin_read_buffer_idx_++; + if (bin_read_buffer_idx_ >= consts.READ_BUFFER_SIZE) bin_read_buffer_idx_ = 0; + } + + int start_idx = 0; + for (int i = 0; i < (int)(bin_read_buffer_idx_ - consts.IMU_BIN_DATA_SIZE); i++) { + if (imu_data_buf[i] == 0xff) { + start_idx = i; + break; + } + } + + if (imu_data_buf[start_idx + consts.IMU_BIN_HEADER_FF0] == 0xff && + imu_data_buf[start_idx + consts.IMU_BIN_HEADER_FF1] == 0xff && + imu_data_buf[start_idx + consts.IMU_BIN_HEADER_R] == 'R' && + imu_data_buf[start_idx + consts.IMU_BIN_HEADER_T] == 'T' && + imu_data_buf[start_idx + consts.IMU_BIN_HEADER_ID0] == 0x39 && + imu_data_buf[start_idx + consts.IMU_BIN_HEADER_ID1] == 0x41) { return true; } return false; } -bool RtUsb9axisimuRosDriver::readBinaryData(void) +RtUsb9axisimuRosDriver::ReadStatus RtUsb9axisimuRosDriver::readBinaryData(void) { static std::vector imu_binary_data_buffer; - unsigned char read_data_buf[256]; + unsigned char read_data_buf[consts.READ_BUFFER_SIZE]; has_refreshed_imu_data_ = false; - int read_data_size = readFromDevice(read_data_buf, - consts.IMU_BIN_DATA_SIZE - imu_binary_data_buffer.size()); + int read_data_size = serial_port_->readFromDevice(read_data_buf, sizeof(read_data_buf)); if(read_data_size == 0){ // The device was unplugged. - return false; + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; } if(read_data_size < 0){ // read() returns error code. - if(errno == EAGAIN || errno == EWOULDBLOCK){ // Wainting for data. - return true; + if(errno == EAGAIN || errno == EWOULDBLOCK){ // Waiting for data. + return RtUsb9axisimuRosDriver::ReadStatus::NEED_TO_CONTINUE; }else{ - return false; + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; } } - for(int i = 0; i < read_data_size; i++){ - imu_binary_data_buffer.push_back(read_data_buf[i]); + int buf_start_idx = 0; + for(int i = 0; i < read_data_size-consts.IMU_BIN_DATA_SIZE+1; i++) { + if(read_data_buf[i] == 0xff && read_data_buf[i+1] == 0xff && + read_data_buf[i+2] == 'R' && read_data_buf[i+3] == 'T') { + buf_start_idx = i; + } + } + + for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++){ + imu_binary_data_buffer.push_back(read_data_buf[i+buf_start_idx]); } if (imu_binary_data_buffer.size() < consts.IMU_BIN_DATA_SIZE){ - return true; + return RtUsb9axisimuRosDriver::ReadStatus::NEED_TO_CONTINUE; } - if (isBinarySensorData(imu_binary_data_buffer.data()) == false) { + if (isBinarySensorData(imu_binary_data_buffer.data(), imu_binary_data_buffer.size()) == false) { imu_binary_data_buffer.clear(); - return false; + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; } auto imu_rawdata = extractBinarySensorData(imu_binary_data_buffer.data()); @@ -134,7 +158,45 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void) sensor_data_.convertRawDataUnit(); // Convert raw data to physical quantity has_refreshed_imu_data_ = true; - return true; + return RtUsb9axisimuRosDriver::ReadStatus::SUCCESS; +} + +bool RtUsb9axisimuRosDriver::isAsciiSensorData(unsigned char * imu_data_buf, unsigned int data_size) +{ + for (int i = 0; i < (int)data_size; i++) { + ascii_read_buffer_[ascii_read_buffer_idx_] = imu_data_buf[i]; + ascii_read_buffer_idx_++; + if (ascii_read_buffer_idx_ >= consts.READ_BUFFER_SIZE) ascii_read_buffer_idx_ = 0; + } + + // convert imu data to vector in ascii format + std::vector> data_vector_ascii; + std::vector data_oneset_ascii; + std::string data_oneline_ascii; + for (int char_count = 0; char_count < (int)ascii_read_buffer_idx_; char_count++) { + if (ascii_read_buffer_[char_count] == '\n') { + data_oneset_ascii.push_back(data_oneline_ascii); + data_vector_ascii.push_back(data_oneset_ascii); + data_oneline_ascii.clear(); + data_oneset_ascii.clear(); + } + else if (ascii_read_buffer_[char_count] == ',') { + data_oneset_ascii.push_back(data_oneline_ascii); + data_oneline_ascii.clear(); + } else { + data_oneline_ascii += ascii_read_buffer_[char_count]; + } + } + + // check data is in ascii format + for (int i = 0; i < (int)data_vector_ascii.size(); i++) { + if (data_vector_ascii.at(i).size() == consts.IMU_ASCII_DATA_SIZE && + data_vector_ascii.at(i).at(consts.IMU_ASCII_TIMESTAMP).find(".") == std::string::npos && + isValidAsciiSensorData(data_vector_ascii.at(i))) { + return true; + } + } + return false; } bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector str_vector) @@ -147,24 +209,46 @@ bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector str return true; } -bool RtUsb9axisimuRosDriver::readAsciiData(void) +RtUsb9axisimuRosDriver::ReadStatus RtUsb9axisimuRosDriver::readAsciiData(void) { static std::vector imu_data_vector_buf; - unsigned char imu_data_buf[256]; + unsigned char imu_data_buf[consts.READ_BUFFER_SIZE]; rt_usb_9axisimu::ImuData imu_data; std::string imu_data_oneline_buf; has_refreshed_imu_data_ = false; imu_data_oneline_buf.clear(); - int data_size_of_buf = readFromDevice(imu_data_buf, sizeof(imu_data_buf)); + int data_size_of_buf = serial_port_->readFromDevice(imu_data_buf, sizeof(imu_data_buf)); if (data_size_of_buf <= 0) { - return false; // Raise communication error + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; // Raise communication error } - for (int char_count = 0; char_count < data_size_of_buf; char_count++) { + if(data_size_of_buf < 0){ // read() returns error code. + if(errno == EAGAIN || errno == EWOULDBLOCK){ // Waiting for data. + return RtUsb9axisimuRosDriver::ReadStatus::NEED_TO_CONTINUE; + }else{ + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; + } + } + + int buf_start_idx = 0; + bool newline_flag = false; + for (int i = data_size_of_buf-1; i >= 0; i--) { + if(imu_data_buf[i] == '\n') { + if(newline_flag) { + buf_start_idx = i; + break; + } + else if(!newline_flag) { + newline_flag = true; + } + } + } + + for (int char_count = buf_start_idx; char_count < data_size_of_buf; char_count++) { if (imu_data_buf[char_count] == ',' || imu_data_buf[char_count] == '\n') { imu_data_vector_buf.push_back(imu_data_oneline_buf); // If the imu_data_oneline_buf is empty string (such as receiving @@ -201,13 +285,19 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) } } - return true; + return RtUsb9axisimuRosDriver::ReadStatus::SUCCESS; } RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "") -: rt_usb_9axisimu::SerialPort(port.c_str()) { - has_completed_format_check_ = false; + serial_port_ = std::make_unique(port.c_str()); + data_format_ = DataFormat::NONE; + has_refreshed_imu_data_ = false; +} + +RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::unique_ptr serial_port) +{ + serial_port_ = std::move(serial_port); data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; } @@ -223,7 +313,7 @@ void RtUsb9axisimuRosDriver::setImuFrameIdName(std::string frame_id) void RtUsb9axisimuRosDriver::setImuPortName(std::string port) { - setPort(port.c_str()); + serial_port_->setPort(port.c_str()); } void RtUsb9axisimuRosDriver::setImuStdDev( @@ -238,39 +328,42 @@ void RtUsb9axisimuRosDriver::setImuStdDev( bool RtUsb9axisimuRosDriver::startCommunication() { // returns serial port open status - return openSerialPort(); + return serial_port_->openSerialPort(); } void RtUsb9axisimuRosDriver::stopCommunication(void) { - closeSerialPort(); - has_completed_format_check_ = false; + serial_port_->closeSerialPort(); data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; } -void RtUsb9axisimuRosDriver::checkDataFormat(void) +void RtUsb9axisimuRosDriver::checkDataFormat(const double timeout) { - if (data_format_ == DataFormat::NONE) { - unsigned char data_buf[256]; - int data_size_of_buf = readFromDevice(data_buf, consts.IMU_BIN_DATA_SIZE); - if (data_size_of_buf == consts.IMU_BIN_DATA_SIZE) { - if (isBinarySensorData(data_buf)) { - data_format_ = DataFormat::BINARY; - has_completed_format_check_ = true; - } else { - data_format_ = DataFormat::NOT_BINARY; - } + const auto start_time = std::chrono::system_clock::now(); + while (data_format_ == DataFormat::NONE) { + const auto end_time = std::chrono::system_clock::now(); + const double time_elapsed = (double)std::chrono::duration_cast(end_time - start_time).count(); + if (time_elapsed > timeout) { + return; } - } else if (data_format_ == DataFormat::NOT_BINARY) { - data_format_ = DataFormat::ASCII; - has_completed_format_check_ = true; - } -} + + unsigned char read_buffer[consts.READ_BUFFER_SIZE]; + const auto read_size = serial_port_->readFromDevice(read_buffer, sizeof(read_buffer)); -bool RtUsb9axisimuRosDriver::hasCompletedFormatCheck(void) -{ - return has_completed_format_check_; + if (read_size <= 0) { + continue; + } + + if (isBinarySensorData(read_buffer, read_size)) { + data_format_ = DataFormat::BINARY; + return; + } + else if (isAsciiSensorData(read_buffer, read_size)) { + data_format_ = DataFormat::ASCII; + return; + } + } } bool RtUsb9axisimuRosDriver::hasAsciiDataFormat(void) @@ -368,7 +461,7 @@ std::unique_ptr RtUsb9axisimuRosDriver::getImuTemperatur // Method to receive IMU data, convert those units to SI, and publish to ROS // topic -bool RtUsb9axisimuRosDriver::readSensorData() +RtUsb9axisimuRosDriver::ReadStatus RtUsb9axisimuRosDriver::readSensorData() { if (data_format_ == DataFormat::BINARY) { return readBinaryData(); @@ -376,5 +469,5 @@ bool RtUsb9axisimuRosDriver::readSensorData() return readAsciiData(); } - return false; + return RtUsb9axisimuRosDriver::ReadStatus::FAILURE; } diff --git a/src/rt_usb_9axisimu_driver_component.cpp b/src/rt_usb_9axisimu_driver_component.cpp index 2f38617..6a846c6 100644 --- a/src/rt_usb_9axisimu_driver_component.cpp +++ b/src/rt_usb_9axisimu_driver_component.cpp @@ -65,15 +65,22 @@ Driver::Driver(const rclcpp::NodeOptions & options) void Driver::on_publish_timer() { - if (driver_->readSensorData()) { - if (driver_->hasRefreshedImuData()) { + // Keep reading to get the latest data + auto read_result = RtUsb9axisimuRosDriver::ReadStatus::NEED_TO_CONTINUE; + while (read_result != RtUsb9axisimuRosDriver::ReadStatus::SUCCESS) { + read_result = driver_->readSensorData(); + if (read_result == RtUsb9axisimuRosDriver::ReadStatus::NEED_TO_CONTINUE) { + continue; + } else if (read_result == RtUsb9axisimuRosDriver::ReadStatus::SUCCESS) { rclcpp::Time timestamp = this->now(); imu_data_raw_pub_->publish(std::move(driver_->getImuRawDataUniquePtr(timestamp))); imu_mag_pub_->publish(std::move(driver_->getImuMagUniquePtr(timestamp))); imu_temperature_pub_->publish(std::move(driver_->getImuTemperatureUniquePtr())); + break; + } else if (read_result == RtUsb9axisimuRosDriver::ReadStatus::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices."); + return; } - } else { - RCLCPP_ERROR(this->get_logger(), "readSensorData() returns false, please check your devices."); } } @@ -93,21 +100,16 @@ CallbackReturn Driver::on_configure(const rclcpp_lifecycle::State &) return CallbackReturn::FAILURE; } - while (rclcpp::ok() && driver_->hasCompletedFormatCheck() == false) { - driver_->checkDataFormat(); - } + driver_->checkDataFormat(); - if (rclcpp::ok() && driver_->hasCompletedFormatCheck()) { - RCLCPP_INFO(this->get_logger(), "Format check has completed."); - if (driver_->hasAsciiDataFormat()) { - RCLCPP_INFO(this->get_logger(), "Data format is ascii."); - } else if (driver_->hasBinaryDataFormat()) { - RCLCPP_INFO(this->get_logger(), "Data format is binary."); - } else { - RCLCPP_INFO(this->get_logger(), "Data format is neither binary nor ascii."); - driver_->stopCommunication(); - return CallbackReturn::FAILURE; - } + if (driver_->hasAsciiDataFormat()) { + RCLCPP_INFO(this->get_logger(), "Data format is ascii."); + } else if (driver_->hasBinaryDataFormat()) { + RCLCPP_INFO(this->get_logger(), "Data format is binary."); + } else { + RCLCPP_WARN(this->get_logger(), "Data format is neither binary nor ascii."); + driver_->stopCommunication(); + return CallbackReturn::FAILURE; } imu_data_raw_pub_ = create_publisher("imu/data_raw", 1); @@ -124,8 +126,8 @@ CallbackReturn Driver::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_activate() is called."); - if (!driver_->readSensorData()) { - RCLCPP_ERROR(this->get_logger(), "readSensorData() returns false, please check your devices."); + if (driver_->readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices."); return CallbackReturn::ERROR; } imu_data_raw_pub_->on_activate(); diff --git a/test/test_driver.cpp b/test/test_driver.cpp new file mode 100644 index 0000000..f303765 --- /dev/null +++ b/test/test_driver.cpp @@ -0,0 +1,621 @@ +/* + * test_driver.cpp + * + * License: BSD-3-Clause + * + * Copyright (c) 2015-2023 RT Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of RT Corporation nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "fakeit.hpp" +#include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp" +#include "rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp" + +using fakeit::Mock; +using fakeit::When; +using rt_usb_9axisimu::SerialPort; + +unsigned char int16_to_byte_low_8bit(int16_t val); +unsigned char int16_to_byte_high_8bit(int16_t val); +unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid); +unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid, + int16_t *gyro, int16_t *acc, int16_t *mag, int16_t temp); +std::string double_to_string(double val); +unsigned int create_dummy_ascii_imu_data(unsigned char *buf, bool is_invalid); +unsigned int create_dummy_ascii_imu_data(unsigned char *buf, bool is_invalid, + double *gyro, double *acc, double *mag, double temp); + +class ReadBinaryTestParam { +public: + int16_t gyro[3], acc[3], mag[3], temp; + double ans_gyro[3], ans_acc[3], ans_mag[3], ans_temp; + + ReadBinaryTestParam (int case_num) { + if (case_num == 0) set_data(0, 0.0, 0.0, 0.0, 21.0); // zero + else if (case_num == 1) set_data(32767, 34.87147, 156.90161, 0.00492, 119.14299); // max + else if (case_num == 2) set_data(-32768, -34.87253, -156.9064, -0.00492, -77.14598); // min + else if (case_num == 3) set_data(32766, 34.870401, 156.896823, 0.004915, 119.139995); // boundary + else if (case_num == 4) set_data(-32767, -34.871466, -156.901612, -0.004915, -77.14299); // boundary + else if (case_num == 5) set_data(1, 0.001064, 0.004788, 0.0, 21.002995); // random + else if (case_num == 6) set_data(-1, -0.001064, -0.004788, 0.0, 20.997005); // random + else if (case_num == 7) set_data(999, 1.063161, 4.783615, 0.00015, 23.992183); // random + else if (case_num == 8) set_data(-999, -1.063161, -4.783615, -0.00015, 18.007817); // random + else if (case_num == 9) set_data(12345, 13.13786, 59.112839, 0.001852, 57.975469); // random + else if (case_num == 10) set_data(-12345, -13.13786, -59.112839, -0.001852, -15.975469); // random + } + +private: + void set_data (int16_t test_val, + double ans_gyro_val, double ans_acc_val, double ans_mag_val, double ans_temp_val) { + gyro[0] = gyro[1] = gyro[2] = test_val; + acc[0] = acc[1] = acc[2] = test_val; + mag[0] = mag[1] = mag[2] = test_val; + temp = test_val; + ans_gyro[0] = ans_gyro[1] = ans_gyro[2] = ans_gyro_val; + ans_acc[0] = ans_acc[1] = ans_acc[2] = ans_acc_val; + ans_mag[0] = ans_mag[1] = ans_mag[2] = ans_mag_val; + ans_temp = ans_temp_val; + } +}; + +class ReadAsciiTestParam { +public: + double gyro[3], acc[3], mag[3], temp; + double ans_gyro[3], ans_acc[3], ans_mag[3], ans_temp; + + ReadAsciiTestParam (int case_num) { + if(case_num == 0) set_data(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // zero + else if(case_num == 1) set_data(34.906585, 16.0, 4800.0, 85.0, 156.9064, 0.0048); // max + else if(case_num == 2) set_data(-34.906585, -16.0, -4800.0, -40.0, -156.9064, -0.0048); // min + else if(case_num == 3) set_data(0.00107, 0.0005, 0.14649, 0.0026, 0.00479, 0.00000015); // resolution + else if(case_num == 4) set_data(-0.00107, -0.0005, -0.14649, -0.00122, -0.00479, -0.00000015); // resolution + else if(case_num == 5) set_data(0.1, 0.1, 0.1, 0.1, 0.98067, 0.0000001); // random + else if(case_num == 6) set_data(-0.1, -0.1, -0.1, -0.1, -0.98067, 0.0000001); // random + else if(case_num == 7) set_data(1.0, 1.0, 1.0, 1.0, 9.80665, 0.000001); // random + else if(case_num == 8) set_data(-1.0, -1.0, -1.0, -1.0, -9.80665, -0.000001); // random + else if(case_num == 9) set_data(12.34567, 12.34567, 12.34567, 12.34567, 121.06966, 0.00001); // random + else if(case_num == 10) set_data(-12.34567, -12.34567, -12.34567, -12.34567, -121.06966, -0.00001); // random + } + +private: + void set_data (double gyro_val, double acc_val, double mag_val, double temp_val, + double ans_acc_val, double ans_mag_val) { + gyro[0] = gyro[1] = gyro[2] = gyro_val; + acc[0] = acc[1] = acc[2] = acc_val; + mag[0] = mag[1] = mag[2] = mag_val; + temp = temp_val; + ans_gyro[0] = ans_gyro[1] = ans_gyro[2] = gyro_val; + ans_acc[0] = ans_acc[1] = ans_acc[2] = ans_acc_val; + ans_mag[0] = ans_mag[1] = ans_mag[2] = ans_mag_val; + ans_temp = temp_val; + } +}; + +Mock create_serial_port_mock(void) { + Mock mock; + When(Method(mock, setPort)).AlwaysReturn(); + When(Method(mock, openPort)).AlwaysReturn(true); + When(Method(mock, openSerialPort)).AlwaysReturn(true); + When(Method(mock, closeSerialPort)).AlwaysReturn(); + When(Method(mock, readFromDevice)).AlwaysReturn(0); + When(Method(mock, writeToDevice)).AlwaysReturn(0); + return mock; +} + +unsigned char int16_to_byte_low_8bit(int16_t val) { + return (val >> 0) & 0xFF; +} + +unsigned char int16_to_byte_high_8bit(int16_t val) { + return (val >> 8) & 0xFF; +} + +unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid) { + ReadBinaryTestParam data(0); + return create_dummy_bin_imu_data(buf, is_invalid, data.gyro, data.acc, data.mag, data.temp); +} + +unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid, + int16_t *gyro, int16_t *acc, int16_t *mag, int16_t temp) { + rt_usb_9axisimu::Consts consts; + unsigned char dummy_bin_imu_data[consts.IMU_BIN_DATA_SIZE] = {0}; + dummy_bin_imu_data[consts.IMU_BIN_HEADER_FF0] = 0xff; + dummy_bin_imu_data[consts.IMU_BIN_HEADER_FF1] = 0xff; + if (is_invalid) { + dummy_bin_imu_data[consts.IMU_BIN_HEADER_R] = 0x54; // T + dummy_bin_imu_data[consts.IMU_BIN_HEADER_T] = 0x52; // R + } else { + dummy_bin_imu_data[consts.IMU_BIN_HEADER_R] = 0x52; // R + dummy_bin_imu_data[consts.IMU_BIN_HEADER_T] = 0x54; // T + } + dummy_bin_imu_data[consts.IMU_BIN_HEADER_ID0] = 0x39; + dummy_bin_imu_data[consts.IMU_BIN_HEADER_ID1] = 0x41; + dummy_bin_imu_data[consts.IMU_BIN_FIRMWARE] = 0x12; + dummy_bin_imu_data[consts.IMU_BIN_TIMESTAMP] = 0x00; + dummy_bin_imu_data[consts.IMU_BIN_ACC_X_L] = int16_to_byte_low_8bit(acc[0]); + dummy_bin_imu_data[consts.IMU_BIN_ACC_X_H] = int16_to_byte_high_8bit(acc[0]); + dummy_bin_imu_data[consts.IMU_BIN_ACC_Y_L] = int16_to_byte_low_8bit(acc[1]); + dummy_bin_imu_data[consts.IMU_BIN_ACC_Y_H] = int16_to_byte_high_8bit(acc[1]); + dummy_bin_imu_data[consts.IMU_BIN_ACC_Z_L] = int16_to_byte_low_8bit(acc[2]); + dummy_bin_imu_data[consts.IMU_BIN_ACC_Z_H] = int16_to_byte_high_8bit(acc[2]); + dummy_bin_imu_data[consts.IMU_BIN_TEMP_L] = int16_to_byte_low_8bit(temp); + dummy_bin_imu_data[consts.IMU_BIN_TEMP_H] = int16_to_byte_high_8bit(temp); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_X_L] = int16_to_byte_low_8bit(gyro[0]); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_X_H] = int16_to_byte_high_8bit(gyro[0]); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_Y_L] = int16_to_byte_low_8bit(gyro[1]); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_Y_H] = int16_to_byte_high_8bit(gyro[1]); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_Z_L] = int16_to_byte_low_8bit(gyro[2]); + dummy_bin_imu_data[consts.IMU_BIN_GYRO_Z_H] = int16_to_byte_high_8bit(gyro[2]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_X_L] = int16_to_byte_low_8bit(mag[0]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_X_H] = int16_to_byte_high_8bit(mag[0]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_Y_L] = int16_to_byte_low_8bit(mag[1]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_Y_H] = int16_to_byte_high_8bit(mag[1]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_Z_L] = int16_to_byte_low_8bit(mag[2]); + dummy_bin_imu_data[consts.IMU_BIN_MAG_Z_H] = int16_to_byte_high_8bit(mag[2]); + for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++) { + buf[i] = dummy_bin_imu_data[i]; + } + return consts.IMU_BIN_DATA_SIZE; +} + +std::string double_to_string(double val) { + std::string str = std::to_string(val); + str.resize(8, '0'); + return str; +} + +unsigned int create_dummy_ascii_imu_data(unsigned char *buf, bool is_invalid) { + ReadAsciiTestParam data(0); + return create_dummy_ascii_imu_data(buf, is_invalid, data.gyro, data.acc, data.mag, data.temp); +} + +unsigned int create_dummy_ascii_imu_data(unsigned char *buf, bool is_invalid, + double *gyro, double *acc, double *mag, double temp) { + rt_usb_9axisimu::Consts consts; + std::vector dummy_ascii_imu_data(consts.IMU_ASCII_DATA_SIZE); + if (is_invalid) { + dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0.0"; + } else { + dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0"; + } + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = double_to_string(gyro[0]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = double_to_string(gyro[1]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = double_to_string(gyro[2]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = double_to_string(acc[0]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = double_to_string(acc[1]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = double_to_string(acc[2]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = double_to_string(mag[0]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = double_to_string(mag[1]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = double_to_string(mag[2]).c_str(); + dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = double_to_string(temp).c_str(); + const char split_char = ','; + const char newline_char = '\n'; + buf[0] = (unsigned char)newline_char; + unsigned int char_count = 1; + for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) { + for(int j = 0; j < (int)strlen(dummy_ascii_imu_data.at(i)); j++) { + buf[char_count] = (unsigned char)dummy_ascii_imu_data.at(i)[j]; + char_count++; + } + if(i != consts.IMU_ASCII_DATA_SIZE - 1) buf[char_count] = (unsigned char)split_char; + else buf[char_count] = (unsigned char)newline_char; + char_count++; + } + return char_count; +} + +TEST(TestDriver, startCommunication) +{ + // Expect the startCommunication method to be called twice and return true then false + auto mock = create_serial_port_mock(); + When(Method(mock, openSerialPort)).Return(true, false); // Return true then false + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + EXPECT_TRUE(driver.startCommunication()); + EXPECT_FALSE(driver.startCommunication()); +} + +TEST(TestDriver, initialize_member_variables) +{ + // Expect member variables of the driver instance to be initialised + auto mock = create_serial_port_mock(); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + EXPECT_FALSE(driver.hasBinaryDataFormat()); + EXPECT_FALSE(driver.hasAsciiDataFormat()); + EXPECT_FALSE(driver.hasRefreshedImuData()); +} + +TEST(TestDriver, checkDataFormat_Binary) +{ + // Expect to check correctly when read data in binary format + auto mock = create_serial_port_mock(); + + // 1st: invalid binary data ('R' and 'T' positions are reversed) + // 2nd: correct binary data ('R' and 'T' are in the correct position) + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, true); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, false); + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + + EXPECT_TRUE(driver.hasBinaryDataFormat()); + EXPECT_FALSE(driver.hasAsciiDataFormat()); +} + +TEST(TestDriver, checkDataFormat_ASCII) +{ + // Expect to check correctly when read data in ASCII format + auto mock = create_serial_port_mock(); + + // 1st: invalid ascii data (timestamp is double) + // 2nd: correct ascii data (timestamp is int) + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, true); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, false); + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + + EXPECT_TRUE(driver.hasAsciiDataFormat()); + EXPECT_FALSE(driver.hasBinaryDataFormat()); +} + +TEST(TestDriver, checkDataFormat_not_Binary_or_ASCII) +{ + // Expect to check correctly when read data in not Binary or ASCII format + auto mock = create_serial_port_mock(); + + // always invalid data (not binary or ascii) + When(Method(mock, readFromDevice)).AlwaysDo([]( + unsigned char* buf, unsigned int buf_size) { + unsigned char dummy_data_not_binary_or_ascii[] = + "dummy_data_not_binary_or_ascii"; + for(int i = 0; i < (int)sizeof(dummy_data_not_binary_or_ascii); i++) { + buf[i] = dummy_data_not_binary_or_ascii[i]; + } + buf_size = strlen((char*)buf); + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + + EXPECT_FALSE(driver.hasBinaryDataFormat()); + EXPECT_FALSE(driver.hasAsciiDataFormat()); +} + +TEST(TestDriver, readSensorData_Binary) +{ + // Expect to check the data is correctly updated when binary data is read + auto mock = create_serial_port_mock(); + + // 1st: correct binary data ('R' and 'T' are in the correct position) + // 2nd: invalid binary data ('R' and 'T' positions are reversed) + // 3rd: correct binary data ('R' and 'T' are in the correct position) + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, false); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, true); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, false); + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); // 1st + driver.readSensorData(); // 2nd + + EXPECT_FALSE(driver.hasRefreshedImuData()); + + driver.readSensorData(); // 3rd + + EXPECT_TRUE(driver.hasRefreshedImuData()); +} + +TEST(TestDriver, readSensorData_ASCII) +{ + // Expect to check the data is correctly updated when ascii data is read + auto mock = create_serial_port_mock(); + + // 1st: correct ascii data (timestamp is int) + // 2nd: invalid ascii data (timestamp is double) + // 3rd: correct ascii data (timestamp is int) + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, false); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, true); + return buf_size; + }).Do([]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, false); + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); // 1st + driver.readSensorData(); // 2nd + + EXPECT_FALSE(driver.hasRefreshedImuData()); + + driver.readSensorData(); // 3rd + + EXPECT_TRUE(driver.hasRefreshedImuData()); +} + +class ReadBinaryTest : public testing::TestWithParam { +}; + +TEST_P(ReadBinaryTest, read_binary_test) { + // Expect to check the data is correctly converted when binary data is read + auto mock = create_serial_port_mock(); + auto data = GetParam(); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + When(Method(mock, readFromDevice)).AlwaysDo([&]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_bin_imu_data(buf, false, data.gyro, data.acc, data.mag, data.temp); + return buf_size; + }); + + driver.checkDataFormat(); + driver.readSensorData(); + + rclcpp::Time timestamp; + auto imu_data_raw = driver.getImuRawDataUniquePtr(timestamp); + auto imu_data_mag = driver.getImuMagUniquePtr(timestamp); + auto imu_data_temperature = driver.getImuTemperatureUniquePtr(); + + const double abs_error_acc = 1e-3; + const double abs_error_gyro = 1e-3; + const double abs_error_mag = 1e-5; + const double abs_error_temp = 1e-3; + EXPECT_NEAR(imu_data_raw->linear_acceleration.x, data.ans_acc[0], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.y, data.ans_acc[1], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.z, data.ans_acc[2], abs_error_acc); + EXPECT_NEAR(imu_data_raw->angular_velocity.x, data.ans_gyro[0], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.y, data.ans_gyro[1], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.z, data.ans_gyro[2], abs_error_gyro); + EXPECT_NEAR(imu_data_mag->magnetic_field.x, data.ans_mag[0], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.y, data.ans_mag[1], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.z, data.ans_mag[2], abs_error_mag); + EXPECT_NEAR(imu_data_temperature->data, data.ans_temp, abs_error_temp); +} + +INSTANTIATE_TEST_SUITE_P( + TestDriver, + ReadBinaryTest, + ::testing::Values( + ReadBinaryTestParam(0), + ReadBinaryTestParam(1), + ReadBinaryTestParam(2), + ReadBinaryTestParam(3), + ReadBinaryTestParam(4), + ReadBinaryTestParam(5), + ReadBinaryTestParam(6), + ReadBinaryTestParam(7), + ReadBinaryTestParam(8), + ReadBinaryTestParam(9), + ReadBinaryTestParam(10) + ) +); + +class ReadAsciiTest : public testing::TestWithParam { +}; + +TEST_P(ReadAsciiTest, read_ascii_test) { + // Expect to check the data is correctly converted when ascii data is read + auto mock = create_serial_port_mock(); + auto data = GetParam(); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + When(Method(mock, readFromDevice)).AlwaysDo([&]( + unsigned char* buf, unsigned int buf_size) { + buf_size = create_dummy_ascii_imu_data(buf, false, data.gyro, data.acc, data.mag, data.temp); + return buf_size; + }); + + driver.checkDataFormat(); + driver.readSensorData(); + + rclcpp::Time timestamp; + auto imu_data_raw = driver.getImuRawDataUniquePtr(timestamp); + auto imu_data_mag = driver.getImuMagUniquePtr(timestamp); + auto imu_data_temperature = driver.getImuTemperatureUniquePtr(); + + const double abs_error_acc = 1e-3; + const double abs_error_gyro = 1e-3; + const double abs_error_mag = 1e-5; + const double abs_error_temp = 1e-3; + EXPECT_NEAR(imu_data_raw->linear_acceleration.x, data.ans_acc[0], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.y, data.ans_acc[1], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.z, data.ans_acc[2], abs_error_acc); + EXPECT_NEAR(imu_data_raw->angular_velocity.x, data.ans_gyro[0], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.y, data.ans_gyro[1], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.z, data.ans_gyro[2], abs_error_gyro); + EXPECT_NEAR(imu_data_mag->magnetic_field.x, data.ans_mag[0], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.y, data.ans_mag[1], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.z, data.ans_mag[2], abs_error_mag); + EXPECT_NEAR(imu_data_temperature->data, data.ans_temp, abs_error_temp); +} + +INSTANTIATE_TEST_SUITE_P( + TestDriver, + ReadAsciiTest, + ::testing::Values( + ReadAsciiTestParam(0), + ReadAsciiTestParam(1), + ReadAsciiTestParam(2), + ReadAsciiTestParam(3), + ReadAsciiTestParam(4), + ReadAsciiTestParam(5), + ReadAsciiTestParam(6), + ReadAsciiTestParam(7), + ReadAsciiTestParam(8), + ReadAsciiTestParam(9), + ReadAsciiTestParam(10) + ) +); + +TEST(TestDriver, get_latest_data_Binary) { + // Expect to check the latest data is correctly read when binary sequential data is read + auto mock = create_serial_port_mock(); + const int MAX_CASE_NUM = 3; + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + When(Method(mock, readFromDevice)).AlwaysDo([&]( + unsigned char* buf, unsigned int buf_size) { + rt_usb_9axisimu::Consts consts; + buf_size = 0; + for(int i = 0; i < MAX_CASE_NUM; i++) { + unsigned char oneset_data[consts.READ_BUFFER_SIZE]; + ReadBinaryTestParam data(i); + auto oneset_size = create_dummy_bin_imu_data(oneset_data, false, data.gyro, data.acc, data.mag, data.temp); + for(unsigned int j = 0; j < oneset_size; j++) { + buf[j+buf_size] = oneset_data[j]; + } + buf_size += oneset_size; + } + return buf_size; + }); + + driver.checkDataFormat(); + driver.readSensorData(); + + rclcpp::Time timestamp; + auto imu_data_raw = driver.getImuRawDataUniquePtr(timestamp); + auto imu_data_mag = driver.getImuMagUniquePtr(timestamp); + auto imu_data_temperature = driver.getImuTemperatureUniquePtr(); + + ReadBinaryTestParam data(MAX_CASE_NUM-1); + const double abs_error_acc = 1e-3; + const double abs_error_gyro = 1e-3; + const double abs_error_mag = 1e-5; + const double abs_error_temp = 1e-3; + EXPECT_NEAR(imu_data_raw->linear_acceleration.x, data.ans_acc[0], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.y, data.ans_acc[1], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.z, data.ans_acc[2], abs_error_acc); + EXPECT_NEAR(imu_data_raw->angular_velocity.x, data.ans_gyro[0], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.y, data.ans_gyro[1], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.z, data.ans_gyro[2], abs_error_gyro); + EXPECT_NEAR(imu_data_mag->magnetic_field.x, data.ans_mag[0], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.y, data.ans_mag[1], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.z, data.ans_mag[2], abs_error_mag); + EXPECT_NEAR(imu_data_temperature->data, data.ans_temp, abs_error_temp); +} + +TEST(TestDriver, get_latest_data_ASCII) { + // Expect to check the latest data is correctly read when ascii sequential data is read + auto mock = create_serial_port_mock(); + const int MAX_CASE_NUM = 3; + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + When(Method(mock, readFromDevice)).AlwaysDo([&]( + unsigned char* buf, unsigned int buf_size) { + rt_usb_9axisimu::Consts consts; + buf_size = 0; + for(int i = 0; i < MAX_CASE_NUM; i++) { + unsigned char oneset_data[consts.READ_BUFFER_SIZE]; + ReadAsciiTestParam data(i); + auto oneset_size = create_dummy_ascii_imu_data(oneset_data, false, data.gyro, data.acc, data.mag, data.temp); + for(unsigned int j = 0; j < oneset_size; j++) { + buf[j+buf_size] = oneset_data[j]; + } + buf_size += oneset_size; + } + return buf_size; + }); + + driver.checkDataFormat(); + driver.readSensorData(); + + rclcpp::Time timestamp; + auto imu_data_raw = driver.getImuRawDataUniquePtr(timestamp); + auto imu_data_mag = driver.getImuMagUniquePtr(timestamp); + auto imu_data_temperature = driver.getImuTemperatureUniquePtr(); + + ReadAsciiTestParam data(MAX_CASE_NUM-1); + const double abs_error_acc = 1e-3; + const double abs_error_gyro = 1e-3; + const double abs_error_mag = 1e-5; + const double abs_error_temp = 1e-3; + EXPECT_NEAR(imu_data_raw->linear_acceleration.x, data.ans_acc[0], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.y, data.ans_acc[1], abs_error_acc); + EXPECT_NEAR(imu_data_raw->linear_acceleration.z, data.ans_acc[2], abs_error_acc); + EXPECT_NEAR(imu_data_raw->angular_velocity.x, data.ans_gyro[0], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.y, data.ans_gyro[1], abs_error_gyro); + EXPECT_NEAR(imu_data_raw->angular_velocity.z, data.ans_gyro[2], abs_error_gyro); + EXPECT_NEAR(imu_data_mag->magnetic_field.x, data.ans_mag[0], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.y, data.ans_mag[1], abs_error_mag); + EXPECT_NEAR(imu_data_mag->magnetic_field.z, data.ans_mag[2], abs_error_mag); + EXPECT_NEAR(imu_data_temperature->data, data.ans_temp, abs_error_temp); +} \ No newline at end of file