2.1.0リリースのためにros2-develの更新内容をhumble-develへマージ (#60)

* 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 <[email protected]>

* 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 <[email protected]>

.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 }}

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 <https://github.com/rt-net/rt_usb_9axisimu_driver/issues/58>`_)
+* Add test for readSensorData() (`#57 <https://github.com/rt-net/rt_usb_9axisimu_driver/issues/57>`_)
+* checkDataFormat() and unit test updates (`#54 <https://github.com/rt-net/rt_usb_9axisimu_driver/issues/54>`_)
+* Add unit tests (`#52 <https://github.com/rt-net/rt_usb_9axisimu_driver/issues/52>`_)
+  Co-authored-by: ShotaAk <[email protected]>
+* Contributors: YusukeKato
+
 2.0.2 (2023-01-26)
 ------------------
 * Support Humble (`#42 <https://github.com/rt-net/rt_usb_9axisimu_driver/issues/42>`_)

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()

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;

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<rt_usb_9axisimu::SerialPort> 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<sensor_msgs::msg::Imu> getImuRawDataUniquePtr(const rclcpp::Time timestamp);
+  std::unique_ptr<sensor_msgs::msg::MagneticField> getImuMagUniquePtr(const rclcpp::Time timestamp);
+  std::unique_ptr<std_msgs::msg::Float64> getImuTemperatureUniquePtr(void);
+  ReadStatus readSensorData();
+
+private:
+  std::unique_ptr<rt_usb_9axisimu::SerialPort> 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,39 +99,18 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
     ASCII,
     INCORRECT
   };
-  bool has_completed_format_check_;
   DataFormat data_format_;
   bool has_refreshed_imu_data_;
 
   // Method to combine two separate one-byte data into one two-byte data
   int16_t combineByteData(unsigned char data_h, unsigned char data_l);
   // Method to extract binary sensor data from communication buffer
   rt_usb_9axisimu::ImuData<int16_t> 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<std::string> 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<sensor_msgs::msg::Imu> getImuRawDataUniquePtr(const rclcpp::Time timestamp);
-  std::unique_ptr<sensor_msgs::msg::MagneticField> getImuMagUniquePtr(const rclcpp::Time timestamp);
-  std::unique_ptr<std_msgs::msg::Float64> getImuTemperatureUniquePtr(void);
-  bool readSensorData();
+  ReadStatus readAsciiData(void);
 };
 
 #endif  // RT_USB_9AXISIMU_DRIVER__RT_USB_9AXISIMU_DRIVER_HPP_

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rt_usb_9axisimu_driver</name>
-  <version>2.0.2</version>
+  <version>2.1.0</version>
   <description>The rt_usb_9axisimu_driver package</description>
 
   <maintainer email="[email protected]">RT Corporation</maintainer>