From faf39b0a5a923be09ab25fede1b6d531eee9a866 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 5 Apr 2024 17:58:25 +0900 Subject: [PATCH 1/8] Add unit tests (#52) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 --- .github/workflows/industrial_ci.yml | 2 +- CMakeLists.txt | 20 +- .../rt_usb_9axisimu.hpp | 14 +- .../rt_usb_9axisimu_driver.hpp | 9 +- src/rt_usb_9axisimu_driver.cpp | 22 ++- test/test_driver.cpp | 180 ++++++++++++++++++ 6 files changed, 224 insertions(+), 23 deletions(-) create mode 100644 test/test_driver.cpp diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 984d3d5..168e6a6 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -15,7 +15,7 @@ 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: 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..e0bbad8 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp @@ -178,23 +178,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 +221,7 @@ class SerialPort return fd > 0; } - void closeSerialPort() + virtual void closeSerialPort() { if (port_fd_ > 0) { tcsetattr(port_fd_, TCSANOW, &old_settings_); @@ -230,7 +230,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 +239,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..07f8ed3 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,10 @@ #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_; - - // ros::Publisher imu_data_raw_pub_; - // ros::Publisher imu_mag_pub_; - // ros::Publisher imu_temperature_pub_; + std::unique_ptr serial_port_; rt_usb_9axisimu::SensorData sensor_data_; @@ -85,6 +81,7 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort public: explicit RtUsb9axisimuRosDriver(std::string serialport); + RtUsb9axisimuRosDriver(std::unique_ptr serial_port); ~RtUsb9axisimuRosDriver(); void setImuFrameIdName(std::string frame_id); diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index 934e7a7..cbb6bd2 100644 --- a/src/rt_usb_9axisimu_driver.cpp +++ b/src/rt_usb_9axisimu_driver.cpp @@ -99,7 +99,7 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void) unsigned char read_data_buf[256]; has_refreshed_imu_data_ = false; - int read_data_size = readFromDevice(read_data_buf, + int read_data_size = serial_port_->readFromDevice(read_data_buf, consts.IMU_BIN_DATA_SIZE - imu_binary_data_buffer.size()); if(read_data_size == 0){ // The device was unplugged. @@ -158,7 +158,7 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) 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 @@ -205,8 +205,16 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) } RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "") -: rt_usb_9axisimu::SerialPort(port.c_str()) { + serial_port_ = std::make_unique(port.c_str()); + has_completed_format_check_ = false; + data_format_ = DataFormat::NONE; + has_refreshed_imu_data_ = false; +} + +RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::unique_ptr serial_port) +{ + serial_port_ = std::move(serial_port); has_completed_format_check_ = false; data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; @@ -223,7 +231,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,12 +246,12 @@ void RtUsb9axisimuRosDriver::setImuStdDev( bool RtUsb9axisimuRosDriver::startCommunication() { // returns serial port open status - return openSerialPort(); + return serial_port_->openSerialPort(); } void RtUsb9axisimuRosDriver::stopCommunication(void) { - closeSerialPort(); + serial_port_->closeSerialPort(); has_completed_format_check_ = false; data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; @@ -253,7 +261,7 @@ void RtUsb9axisimuRosDriver::checkDataFormat(void) { if (data_format_ == DataFormat::NONE) { unsigned char data_buf[256]; - int data_size_of_buf = readFromDevice(data_buf, consts.IMU_BIN_DATA_SIZE); + int data_size_of_buf = serial_port_->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; diff --git a/test/test_driver.cpp b/test/test_driver.cpp new file mode 100644 index 0000000..68ff4d2 --- /dev/null +++ b/test/test_driver.cpp @@ -0,0 +1,180 @@ +/* + * 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 +#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; + +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; +} + +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.hasCompletedFormatCheck()); + 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(); + + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + 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_R] = 0x52; // R + dummy_bin_imu_data[consts.IMU_BIN_HEADER_T] = 0x54; // T + for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++) { + buf[i] = dummy_bin_imu_data[i]; + } + buf_size = consts.IMU_BIN_DATA_SIZE; + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + + EXPECT_TRUE(driver.hasCompletedFormatCheck()); + 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(); + + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + rt_usb_9axisimu::Consts consts; + std::array dummy_ascii_imu_data; + dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0"; + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000"; + std::string str = ""; + std::string split_char = ","; + for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) { + str += dummy_ascii_imu_data[i]; + if(i != consts.IMU_ASCII_DATA_SIZE - 1) str += split_char; + } + for(int i = 0; i < int(str.length()); i++) { + buf[i] = str[i]; + } + buf_size = consts.IMU_BIN_DATA_SIZE; + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + driver.checkDataFormat(); + + EXPECT_TRUE(driver.hasCompletedFormatCheck()); + 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(); + + When(Method(mock, readFromDevice)).Do([]( + unsigned char* buf, unsigned int buf_size) { + rt_usb_9axisimu::Consts consts; + 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 = consts.IMU_BIN_DATA_SIZE; + return buf_size; + }); + + RtUsb9axisimuRosDriver driver( + std::unique_ptr(&mock.get())); + + driver.checkDataFormat(); + + EXPECT_FALSE(driver.hasCompletedFormatCheck()); + EXPECT_FALSE(driver.hasBinaryDataFormat()); + EXPECT_FALSE(driver.hasAsciiDataFormat()); +} From b22d0602b631a45d3c8b10cfb57f039ae8e04ba8 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 6 Aug 2024 14:58:53 +0900 Subject: [PATCH 2/8] checkDataFormat() and unit test updates (#54) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Binaryデータ出力時は実機とテストの両方で動作確認完了、ASCIIデータには未対応、デバッグ用出力あり * 実機とテストの両方でBinaryとASCIIの判定に成功、BinaryでもASCIIでもないデータには未対応 * タイムアウト機能を追加&hasCompletedFormatCheck()と関係する変数を削除 * 不正なデータをチェックするようにテストを修正 * readを成功させるためにwhileループにsleep処理を追加 * 必要がなかったため、while文のsleep処理を削除 * テスト用データの作成で重複している箇所を関数化 * RCLCPP_INFOをWARNに変更&不要なコメントを削除 * const autoをできる限り使用 * actions/checkoutのバージョンを3から4に更新 * 読み取ったデータをある程度貯めてからデータ形式を判定するように変更 * 貯めるデータを更新するように修正 * メンバ変数の名前を修正&256を定数化 --- .github/workflows/industrial_ci.yml | 2 +- .../rt_usb_9axisimu.hpp | 2 + .../rt_usb_9axisimu_driver.hpp | 12 +- src/rt_usb_9axisimu_driver.cpp | 116 ++++++++++++----- src/rt_usb_9axisimu_driver_component.cpp | 23 ++-- test/test_driver.cpp | 117 +++++++++++------- 6 files changed, 180 insertions(+), 92 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 168e6a6..042d926 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -19,6 +19,6 @@ jobs: 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/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp index e0bbad8..8176a54 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 = 256; + // Convertor const double CONVERTOR_RAW2G; const double CONVERTOR_RAW2DPS; 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 07f8ed3..08877ea 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp @@ -57,6 +57,11 @@ class RtUsb9axisimuRosDriver 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, @@ -66,7 +71,6 @@ class RtUsb9axisimuRosDriver ASCII, INCORRECT }; - bool has_completed_format_check_; DataFormat data_format_; bool has_refreshed_imu_data_; @@ -74,8 +78,9 @@ class RtUsb9axisimuRosDriver 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 isBinarySensorData(unsigned char * imu_data_buf, unsigned int data_size); bool readBinaryData(void); + bool isAsciiSensorData(unsigned char * imu_data_buf, unsigned int data_size); bool isValidAsciiSensorData(std::vector imu_data_vector_buf); bool readAsciiData(void); @@ -90,8 +95,7 @@ class RtUsb9axisimuRosDriver bool startCommunication(); void stopCommunication(void); - void checkDataFormat(void); - bool hasCompletedFormatCheck(void); + void checkDataFormat(const double timeout = 5.0); bool hasAsciiDataFormat(void); bool hasBinaryDataFormat(void); bool hasRefreshedImuData(void); diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index cbb6bd2..3640b1d 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,10 +82,28 @@ 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; } @@ -96,7 +113,7 @@ bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf) bool 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 = serial_port_->readFromDevice(read_data_buf, @@ -122,7 +139,7 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void) return true; } - 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; } @@ -137,6 +154,44 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void) return true; } +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) { for (int i = 1; i < consts.IMU_ASCII_DATA_SIZE; i++) { @@ -151,7 +206,7 @@ bool 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; @@ -207,7 +262,6 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "") { serial_port_ = std::make_unique(port.c_str()); - has_completed_format_check_ = false; data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; } @@ -215,7 +269,6 @@ RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "") RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::unique_ptr serial_port) { serial_port_ = std::move(serial_port); - has_completed_format_check_ = false; data_format_ = DataFormat::NONE; has_refreshed_imu_data_ = false; } @@ -252,33 +305,36 @@ bool RtUsb9axisimuRosDriver::startCommunication() void RtUsb9axisimuRosDriver::stopCommunication(void) { serial_port_->closeSerialPort(); - has_completed_format_check_ = false; 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 = serial_port_->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) diff --git a/src/rt_usb_9axisimu_driver_component.cpp b/src/rt_usb_9axisimu_driver_component.cpp index 2f38617..105c398 100644 --- a/src/rt_usb_9axisimu_driver_component.cpp +++ b/src/rt_usb_9axisimu_driver_component.cpp @@ -93,21 +93,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); diff --git a/test/test_driver.cpp b/test/test_driver.cpp index 68ff4d2..1ea7bed 100644 --- a/test/test_driver.cpp +++ b/test/test_driver.cpp @@ -31,8 +31,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include #include #include "fakeit.hpp" #include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp" @@ -53,6 +52,60 @@ Mock create_serial_port_mock(void) { return mock; } +unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid) { + 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; + for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++) { + buf[i] = dummy_bin_imu_data[i]; + } + return consts.IMU_BIN_DATA_SIZE; +} + +unsigned int create_dummy_ascii_imu_data(unsigned char *buf, bool is_invalid) { + 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] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000"; + dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000"; + 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 @@ -74,7 +127,6 @@ TEST(TestDriver, initialize_member_variables) RtUsb9axisimuRosDriver driver( std::unique_ptr(&mock.get())); - EXPECT_FALSE(driver.hasCompletedFormatCheck()); EXPECT_FALSE(driver.hasBinaryDataFormat()); EXPECT_FALSE(driver.hasAsciiDataFormat()); EXPECT_FALSE(driver.hasRefreshedImuData()); @@ -85,16 +137,15 @@ 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) { - 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_R] = 0x52; // R - dummy_bin_imu_data[consts.IMU_BIN_HEADER_T] = 0x54; // T - for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++) { - buf[i] = dummy_bin_imu_data[i]; - } - buf_size = consts.IMU_BIN_DATA_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; }); @@ -103,7 +154,6 @@ TEST(TestDriver, checkDataFormat_Binary) driver.checkDataFormat(); - EXPECT_TRUE(driver.hasCompletedFormatCheck()); EXPECT_TRUE(driver.hasBinaryDataFormat()); EXPECT_FALSE(driver.hasAsciiDataFormat()); } @@ -113,31 +163,15 @@ 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) { - rt_usb_9axisimu::Consts consts; - std::array dummy_ascii_imu_data; - dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0"; - dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000"; - std::string str = ""; - std::string split_char = ","; - for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) { - str += dummy_ascii_imu_data[i]; - if(i != consts.IMU_ASCII_DATA_SIZE - 1) str += split_char; - } - for(int i = 0; i < int(str.length()); i++) { - buf[i] = str[i]; - } - buf_size = consts.IMU_BIN_DATA_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; }); @@ -145,9 +179,7 @@ TEST(TestDriver, checkDataFormat_ASCII) std::unique_ptr(&mock.get())); driver.checkDataFormat(); - driver.checkDataFormat(); - EXPECT_TRUE(driver.hasCompletedFormatCheck()); EXPECT_TRUE(driver.hasAsciiDataFormat()); EXPECT_FALSE(driver.hasBinaryDataFormat()); } @@ -157,15 +189,15 @@ 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(); - When(Method(mock, readFromDevice)).Do([]( + // always invalid data (not binary or ascii) + When(Method(mock, readFromDevice)).AlwaysDo([]( unsigned char* buf, unsigned int buf_size) { - rt_usb_9axisimu::Consts consts; 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++) { + 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 = consts.IMU_BIN_DATA_SIZE; + buf_size = strlen((char*)buf); return buf_size; }); @@ -174,7 +206,6 @@ TEST(TestDriver, checkDataFormat_not_Binary_or_ASCII) driver.checkDataFormat(); - EXPECT_FALSE(driver.hasCompletedFormatCheck()); EXPECT_FALSE(driver.hasBinaryDataFormat()); EXPECT_FALSE(driver.hasAsciiDataFormat()); -} +} \ No newline at end of file From c63da0b42a1bf9108b7329b145de9d5897c4c234 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 20 Aug 2024 10:57:54 +0900 Subject: [PATCH 3/8] Add test for readSensorData() (#57) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * readSensorData()実行後のhasRefreshedImuData()の応答のテストを追加 * テストにコメントを追加 * 0.0を入力として与えたテストを追加 * ASCII形式のテストデータを作成する際、引数で値を渡せるように変更 * Binary形式でもテストデータを引数で渡せるように変更 * テストケースを使い回せるように変更 * readSensorData()実行時にセンサデータが正しく変換されたか検証するテストを追加 * デバッグ用の関数を削除 * short intをint16_tに変更 * int16を8bitずつに分ける関数をhighとlowそれぞれ用意 * 既存のテストに影響を与えないようにデータ作成の関数をオーバーロード * set_data関数をprivateに変更 * 変換後の数値を直打ち * data_format_を設定する関数を削除 * 不要な箇所を削除 --- test/test_driver.cpp | 372 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 342 insertions(+), 30 deletions(-) diff --git a/test/test_driver.cpp b/test/test_driver.cpp index 1ea7bed..36d7789 100644 --- a/test/test_driver.cpp +++ b/test/test_driver.cpp @@ -41,6 +41,82 @@ 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(); @@ -52,7 +128,21 @@ Mock create_serial_port_mock(void) { 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; @@ -66,44 +156,78 @@ unsigned int create_dummy_bin_imu_data(unsigned char *buf, bool is_invalid) { } 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) { - 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] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000"; - dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000"; - 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; + 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++; } - return 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) @@ -208,4 +332,192 @@ TEST(TestDriver, checkDataFormat_not_Binary_or_ASCII) EXPECT_FALSE(driver.hasBinaryDataFormat()); EXPECT_FALSE(driver.hasAsciiDataFormat()); -} \ No newline at end of file +} + +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) + ) +); \ No newline at end of file From 6d58d4768ddcddaffb66530f69d0072d20dd3a68 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 21 Aug 2024 16:26:20 +0900 Subject: [PATCH 4/8] Fix to get the latest data (#58) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 最新データ取得のテストを追加 * PR#50を参考に最新のデータを取得できるように変更 * 複数セットのデータ取得時に最新のデータをセットするように変更、テストは通るが挙動がおかしい * 最新データの取得方法を修正 * 取得するデータが適切な長さであることを保証する --- .../rt_usb_9axisimu.hpp | 2 +- .../rt_usb_9axisimu_driver.hpp | 53 ++++++---- src/rt_usb_9axisimu_driver.cpp | 65 ++++++++---- src/rt_usb_9axisimu_driver_component.cpp | 19 ++-- test/test_driver.cpp | 100 +++++++++++++++++- 5 files changed, 190 insertions(+), 49 deletions(-) diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp index 8176a54..f555307 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp @@ -106,7 +106,7 @@ class Consts IMU_ASCII_DATA_SIZE }; - static constexpr int READ_BUFFER_SIZE = 256; + static constexpr int READ_BUFFER_SIZE = 512; // Convertor const double CONVERTOR_RAW2G; 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 08877ea..05a20f8 100644 --- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp +++ b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp @@ -46,6 +46,34 @@ class RtUsb9axisimuRosDriver { +public: + explicit RtUsb9axisimuRosDriver(std::string serialport); + RtUsb9axisimuRosDriver(std::unique_ptr serial_port); + ~RtUsb9axisimuRosDriver(); + + 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_; @@ -79,31 +107,10 @@ class RtUsb9axisimuRosDriver // 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, unsigned int data_size); - bool readBinaryData(void); + 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(std::unique_ptr serial_port); - ~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(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); - bool readSensorData(); + ReadStatus readAsciiData(void); }; #endif // RT_USB_9AXISIMU_DRIVER__RT_USB_9AXISIMU_DRIVER_HPP_ diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp index 3640b1d..3bb4588 100644 --- a/src/rt_usb_9axisimu_driver.cpp +++ b/src/rt_usb_9axisimu_driver.cpp @@ -110,38 +110,45 @@ bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf, un return false; } -bool RtUsb9axisimuRosDriver::readBinaryData(void) +RtUsb9axisimuRosDriver::ReadStatus RtUsb9axisimuRosDriver::readBinaryData(void) { static std::vector imu_binary_data_buffer; unsigned char read_data_buf[consts.READ_BUFFER_SIZE]; has_refreshed_imu_data_ = false; - int read_data_size = serial_port_->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; + } + } + + 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 < read_data_size; i++){ - imu_binary_data_buffer.push_back(read_data_buf[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(), 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()); @@ -151,7 +158,7 @@ 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) @@ -202,7 +209,7 @@ bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector str return true; } -bool RtUsb9axisimuRosDriver::readAsciiData(void) +RtUsb9axisimuRosDriver::ReadStatus RtUsb9axisimuRosDriver::readAsciiData(void) { static std::vector imu_data_vector_buf; @@ -216,10 +223,32 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) 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 + } + + 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; + } } - for (int char_count = 0; char_count < data_size_of_buf; char_count++) { + 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 @@ -256,7 +285,7 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void) } } - return true; + return RtUsb9axisimuRosDriver::ReadStatus::SUCCESS; } RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "") @@ -432,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(); @@ -440,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 105c398..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."); } } @@ -119,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 index 36d7789..f303765 100644 --- a/test/test_driver.cpp +++ b/test/test_driver.cpp @@ -520,4 +520,102 @@ INSTANTIATE_TEST_SUITE_P( ReadAsciiTestParam(9), ReadAsciiTestParam(10) ) -); \ No newline at end of file +); + +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 From 806ca079e96588b4f9a4b0f1ef568198825870df Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 23 Aug 2024 11:46:22 +0900 Subject: [PATCH 5/8] =?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=ABCHANGELOG.rst=E3=81=A8packa?= =?UTF-8?q?ge.xml=E3=82=92=E6=9B=B4=E6=96=B0=20(#59)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * CHANGELOGを更新 * 2.1.0 --- CHANGELOG.rst | 9 +++++++++ package.xml | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) 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/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 From 9138e0dd21ba007d2d00eb1ea7cac7545f3b2e1e Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Thu, 31 Oct 2024 11:12:38 +0900 Subject: [PATCH 6/8] =?UTF-8?q?ROS=202=20Jazzy=E3=81=B8=E3=81=AE=E5=AF=BE?= =?UTF-8?q?=E5=BF=9C=20(#61)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix "ROS_DISTRO" of CI for Jazzy * Add authors * Update README for Jazzy * Modify CI to ensure compatibility with the act tool * Add messages of compiler version message for debug * add ccache setting * Set log level "debug" for CI * comment out env for debug * Replace "ROS_REPO: [ros]" with "ROS_REPO: [main]" for CI of "ROS 2" * Fix undefined behavior by storing `std::string` objects instead of `const char*` pointers Previously, the test code for the 9-axis IMU was storing `const char*` pointers obtained from temporary `std::string` objects into a vector. Since these temporary `std::string` objects are destroyed immediately after the expression, the stored pointers became invalid (dangling pointers), leading to undefined behavior and causing tests to fail. This commit fixes the issue by changing the vector to store *std::string* objects directly instead of `const char*` pointers. By maintaining the `std::string` objects, we ensure that the data remains valid throughout its usage, and any `c_str()` calls on these strings return valid pointers. This change resolves the test failures and prevents potential crashes or incorrect data processing due to invalid memory access. * Update copyright year --- .github/workflows/industrial_ci.yml | 15 ++++++++-- CMakeLists.txt | 4 +++ README.md | 45 +++++++++++------------------ package.xml | 3 +- test/test_driver.cpp | 29 ++++++++++--------- 5 files changed, 50 insertions(+), 46 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 042d926..3355069 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -14,11 +14,20 @@ jobs: industrial_ci: strategy: matrix: - env: - - { ROS_DISTRO: humble, ROS_REPO: ros, AFTER_SETUP_TARGET_WORKSPACE: 'apt update && apt install -y git' } + ROS_DISTRO: [jazzy] + ROS_REPO: [main] + AFTER_SETUP_TARGET_WORKSPACE: [apt update && apt install -y git gcc-11 g++-11] + # env: + # CCACHE_DIR: "${{ github.workspace }}/.ccache" # ccache directory for debug runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - uses: "ros-industrial/industrial_ci@master" - env: ${{ matrix.env }} + env: + ROS_DISTRO: ${{matrix.ROS_DISTRO}} + ROS_REPO: ${{matrix.ROS_REPO}} + AFTER_SETUP_TARGET_WORKSPACE: ${{matrix.AFTER_SETUP_TARGET_WORKSPACE}} + BUILD_TOOL_ARGS: "--event-handlers console_direct+" + COLCON_LOG_LEVEL: debug + VERBOSE_OUTPUT: true diff --git a/CMakeLists.txt b/CMakeLists.txt index 2db4440..4ac4734 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +message(STATUS "CMake Version: ${CMAKE_VERSION}") +message(STATUS "C++ Compiler: ${CMAKE_CXX_COMPILER}") +message(STATUS "C++ Compiler Version: ${CMAKE_CXX_COMPILER_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") # find dependencies find_package(ament_cmake REQUIRED) diff --git a/README.md b/README.md index 137adde..0e16575 100644 --- a/README.md +++ b/README.md @@ -2,30 +2,22 @@ # rt_usb_9axisimu_driver -株式会社アールティが販売しているUSB出力9軸IMUセンサモジュール用のROS 2パッケージです。 +[![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=ros2-devel)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Aros2-devel) + +株式会社アールティが販売しているUSB出力9軸IMUセンサモジュール用のROS 2パッケージです。 ![usb-9axisimu](https://rt-net.github.io/images/usb-9axisimu/usb-9axisimu.png) 現在、以下のROSのディストリビューションに対応しております。 -- Melodic ([`melodic-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/melodic-devel)) -- Noetic ([`noetic-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/noetic-devel)) -- Foxy ([`foxy-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/foxy-devel)) -- Humble ([`humble-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/humble-devel)) - -| | industrial_ci | source build | amd64 binary | arm64 binary | -|:---:|:---:|:---:|:---:|:---:| -| main develop
([`master`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/master)) | [![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=master)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Amaster) | - | - | - | - | -| ROS 2 develop
([`ros2-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/ros2-devel)) | [![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=ros2-devel)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Aros2-devel) | - | - | - | - | -| Bionic + Melodic
([`melodic-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/melodic-devel)) | [![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=melodic-devel)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Amelodic-devel) | [![Build Status](http://build.ros.org/job/Msrc_uB__rt_usb_9axisimu_driver__ubuntu_bionic__source/badge/icon)](http://build.ros.org/job/Msrc_uB__rt_usb_9axisimu_driver__ubuntu_bionic__source/) | [![Build Status](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rt_usb_9axisimu_driver__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rt_usb_9axisimu_driver__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros.org/job/Mbin_ubv8_uBv8__rt_usb_9axisimu_driver__ubuntu_bionic_arm64__binary/badge/icon)](http://build.ros.org/job/Mbin_ubv8_uBv8__rt_usb_9axisimu_driver__ubuntu_bionic_arm64__binary/) | -| Focal + Noetic
([`noetic-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/noetic-devel)) | [![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=noetic-devel)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Anoetic-devel) | - | - | - | - | -| Focal + Foxy
([`foxy-devel`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/foxy-devel)) | [![industrial_ci](https://github.com/rt-net/rt_usb_9axisimu_driver/workflows/industrial_ci/badge.svg?branch=foxy-devel)](https://github.com/rt-net/rt_usb_9axisimu_driver/actions?query=workflow%3Aindustrial_ci+branch%3Afoxy-devel) | [![Build Status](https://build.ros2.org/view/Fsrc_uF/job/Fsrc_uF__rt_usb_9axisimu_driver__ubuntu_focal__source/badge/icon)](https://build.ros2.org/view/Fsrc_uF/job/Fsrc_uF__rt_usb_9axisimu_driver__ubuntu_focal__source/) |[![Build Status](https://build.ros2.org/view/Fsrc_uF/job/Fbin_uF64__rt_usb_9axisimu_driver__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/view/Fsrc_uF/job/Fbin_uF64__rt_usb_9axisimu_driver__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/view/Fbin_ubv8_uFv8/job/Fbin_ubv8_uFv8__rt_usb_9axisimu_driver__ubuntu_focal_arm64__binary/badge/icon)](https://build.ros2.org/view/Fbin_ubv8_uFv8/job/Fbin_ubv8_uFv8__rt_usb_9axisimu_driver__ubuntu_focal_arm64__binary/) | -**TODO: Add Jammy + Humble** + +- ROS 2 Humble ([`humble`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/humble)) +- ROS 2 Jazzy ([`jazzy`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/jazzy)) ## 1. 概要 rt_usb_9axisimu_driverは株式会社アールティが販売している [USB出力9軸IMUセンサモジュール](https://www.rt-net.jp/products/9axisimu2/) -のROS 2パッケージです。 +のROS 2パッケージです。 株式会社アールティによって開発、メンテナンスがなされています。 @@ -33,22 +25,22 @@ rt_usb_9axisimu_driverは株式会社アールティが販売している ### 1.1 座標軸について -USB出力9軸IMUセンサモジュールは、センサとしてInvenSense社のMPU9250を使用しております。 +USB出力9軸IMUセンサモジュールは、センサとしてInvenSense社のMPU9250を使用しております。 このセンサの磁気センサの座標系はNED座標系(x-north, y-east, z-down)ですが、 モジュール内のマイコン(LPC1343)においてENU座標系(x-east, y-north, z-up)に変換され、 -ジャイロセンサおよび加速度センサの座標系と揃えられております。 +ジャイロセンサおよび加速度センサの座標系と揃えられております。 これはROSで使われる座標系のルールにも適合しています。詳しくは、[REP-0103](http://www.ros.org/reps/rep-0103.html#axis-orientation)をご覧ください。 ### 1.2 ファームウェア開発について -USB出力9軸IMUセンサモジュールはオープンハード・オープンソースのため、モジュール内のマイコンのファームウェアの変更が可能です。 +USB出力9軸IMUセンサモジュールはオープンハード・オープンソースのため、モジュール内のマイコンのファームウェアの変更が可能です。 このROSパッケージはデフォルトのファームウェアにのみ対応しております。ファームウェアを変更された場合、正常な動作ができなくなる恐れがございますので、ご了承ください。 ### 1.3 ver2.0でのご利用について -2020年8月現在、販売されているUSB出力9軸IMUセンサモジュールはver2.0となります。 -このバージョンのデフォルトのファームウェアには、ASCII出力とBinary出力の２つのデータ出力形式があります。 -センサ出荷時点ではASCII出力に設定されています。出力形式の切り替え方法は、以下のリポジトリにあるマニュアルをご参照ください。 +2020年8月現在、販売されているUSB出力9軸IMUセンサモジュールはver2.0となります。 +このバージョンのデフォルトのファームウェアには、ASCII出力とBinary出力の２つのデータ出力形式があります。 +センサ出荷時点ではASCII出力に設定されています。出力形式の切り替え方法は、以下のリポジトリにあるマニュアルをご参照ください。 https://github.com/rt-net/RT-USB-9AXIS-00 ### [ERROR] Error opening sensor device, please re-check your devices. が発生する場合 @@ -61,16 +53,13 @@ $ sudo chmod 666 /dev/ttyACM0 ## 2. インストール - -ROS Melodic等ROS 1のパッケージについては[`master`](https://github.com/rt-net/rt_usb_9axisimu_driver/tree/master)ブランチのREADMEをご覧ください。 - ### 2.1 バイナリをインストールする場合 ```sh -# ROS 2 Foxy -$ sudo apt install ros-foxy-rt-usb-9axisimu-driver -# ROS 2 Humble +# ROS 2 Humble $ sudo apt install ros-humble-rt-usb-9axisimu-driver +# ROS 2 Jazzy (ToDo) +$ ``` ### 2.2 ソースからインストールする場合 @@ -78,7 +67,7 @@ $ sudo apt install ros-humble-rt-usb-9axisimu-driver ```sh $ cd ~/ros2_ws/src # Clone package & checkout ROS 2 branch -$ git clone -b $ROS_DISTRO-devel https://github.com/rt-net/rt_usb_9axisimu_driver +$ git clone -b $ROS_DISTRO https://github.com/rt-net/rt_usb_9axisimu_driver # Install dependencies $ rosdep install -r -y -i --from-paths . diff --git a/package.xml b/package.xml index c7c4671..bf29b38 100644 --- a/package.xml +++ b/package.xml @@ -7,6 +7,8 @@ RT Corporation RT Corporation + Yusuke Kato + Kazushi Kurasawa BSD @@ -29,4 +31,3 @@ ament_cmake - diff --git a/test/test_driver.cpp b/test/test_driver.cpp index f303765..b0fe305 100644 --- a/test/test_driver.cpp +++ b/test/test_driver.cpp @@ -3,7 +3,7 @@ * * License: BSD-3-Clause * - * Copyright (c) 2015-2023 RT Corporation + * Copyright (c) 2015-2024 RT Corporation * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -198,29 +198,30 @@ 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) { rt_usb_9axisimu::Consts consts; - std::vector dummy_ascii_imu_data(consts.IMU_ASCII_DATA_SIZE); + 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(); + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = double_to_string(gyro[0]); + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = double_to_string(gyro[1]); + dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = double_to_string(gyro[2]); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = double_to_string(acc[0]); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = double_to_string(acc[1]); + dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = double_to_string(acc[2]); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = double_to_string(mag[0]); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = double_to_string(mag[1]); + dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = double_to_string(mag[2]); + dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = double_to_string(temp); 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]; + const char* data_str = dummy_ascii_imu_data.at(i).c_str(); + for(int j = 0; j < (int)strlen(data_str); j++) { + buf[char_count] = (unsigned char)data_str[j]; char_count++; } if(i != consts.IMU_ASCII_DATA_SIZE - 1) buf[char_count] = (unsigned char)split_char; From 6ffb0a90539778178822b0935593c362948fa174 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Thu, 31 Oct 2024 17:47:17 +0900 Subject: [PATCH 7/8] =?UTF-8?q?ROS=202=20Rolling=E3=81=B8=E3=81=AECI?= =?UTF-8?q?=E5=AF=BE=E5=BF=9C=20(#62)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update CI for ROS 2 Rolling * Summarize setting "AFTER_SETUP_TARGET_WORKSPACE" --- .github/workflows/industrial_ci.yml | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 3355069..bd45cbd 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -14,20 +14,14 @@ jobs: industrial_ci: strategy: matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [main] - AFTER_SETUP_TARGET_WORKSPACE: [apt update && apt install -y git gcc-11 g++-11] - # env: - # CCACHE_DIR: "${{ github.workspace }}/.ccache" # ccache directory for debug + env: + - { ROS_DISTRO: rolling, ROS_REPO: main} + - { ROS_DISTRO: jazzy, ROS_REPO: main} + env: + AFTER_SETUP_TARGET_WORKSPACE: 'apt update && apt install -y git' runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - uses: "ros-industrial/industrial_ci@master" - env: - ROS_DISTRO: ${{matrix.ROS_DISTRO}} - ROS_REPO: ${{matrix.ROS_REPO}} - AFTER_SETUP_TARGET_WORKSPACE: ${{matrix.AFTER_SETUP_TARGET_WORKSPACE}} - BUILD_TOOL_ARGS: "--event-handlers console_direct+" - COLCON_LOG_LEVEL: debug - VERBOSE_OUTPUT: true + env: ${{ matrix.env }} From 3766c9c0f54bbf4fdeb6fa92aacab8a17f9748a9 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Thu, 7 Nov 2024 15:45:24 +0900 Subject: [PATCH 8/8] =?UTF-8?q?=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B93.0.0?= =?UTF-8?q?=E3=81=B8=E3=81=AE=E5=AF=BE=E5=BF=9C=20(#63)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update CHANGELOG.rst * Update release version * Fix PR links * Fix date --- CHANGELOG.rst | 7 +++++++ package.xml | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 2d3c3c9..81b1ae4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rt_usb_9axisimu_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.0 (2024-11-07) +------------------ +* Update CI for ROS 2 Rolling (`https://github.com/rt-net/rt_usb_9axisimu_driver/pull/62`) +* Support for ROS 2 Jazzy (`https://github.com/rt-net/rt_usb_9axisimu_driver/pull/61`) +* Fix undefined behavior by storing `std::string` objects instead of `const char*` pointers +* Contributors: Kazushi Kurasawa, YusukeKato + 2.1.0 (2024-08-23) ------------------ * Fix to get the latest data (`#58 `_) diff --git a/package.xml b/package.xml index bf29b38..159e9fe 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ rt_usb_9axisimu_driver - 2.1.0 + 3.0.0 The rt_usb_9axisimu_driver package RT Corporation pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy