Skip to content

Commit

Permalink
checkDataFormat() and unit test updates (#54)
Browse files Browse the repository at this point in the history 
* Binaryデータ出力時は実機とテストの両方で動作確認完了、ASCIIデータには未対応、デバッグ用出力あり

* 実機とテストの両方でBinaryとASCIIの判定に成功、BinaryでもASCIIでもないデータには未対応

* タイムアウト機能を追加&hasCompletedFormatCheck()と関係する変数を削除

* 不正なデータをチェックするようにテストを修正

* readを成功させるためにwhileループにsleep処理を追加

* 必要がなかったため、while文のsleep処理を削除

* テスト用データの作成で重複している箇所を関数化

* RCLCPP_INFOをWARNに変更&不要なコメントを削除

* const autoをできる限り使用

* actions/checkoutのバージョンを3から4に更新

* 読み取ったデータをある程度貯めてからデータ形式を判定するように変更

* 貯めるデータを更新するように修正

* メンバ変数の名前を修正&256を定数化
  • Loading branch information
@YusukeKato
YusukeKato authored Aug 6, 2024
1 parent faf39b0 commit b22d060
Show file tree
Hide file tree
Showing 6 changed files with 180 additions and 92 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/industrial_ci.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 }}
2 changes: 2 additions & 0 deletions include/rt_usb_9axisimu_driver/rt_usb_9axisimu.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
12 changes: 8 additions & 4 deletions include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -66,16 +71,16 @@ class RtUsb9axisimuRosDriver
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 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<std::string> imu_data_vector_buf);
bool readAsciiData(void);

Expand All @@ -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);
Expand Down
116 changes: 86 additions & 30 deletions src/rt_usb_9axisimu_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <cstring>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <chrono>

#include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp"

Expand Down Expand Up @@ -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;
}
Expand All @@ -96,7 +113,7 @@ bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf)
bool RtUsb9axisimuRosDriver::readBinaryData(void)
{
static std::vector<unsigned char> 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,
Expand All @@ -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;
}
Expand All @@ -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<std::vector<std::string>> data_vector_ascii;
std::vector<std::string> 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<std::string> str_vector)
{
for (int i = 1; i < consts.IMU_ASCII_DATA_SIZE; i++) {
Expand All @@ -151,7 +206,7 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void)
{
static std::vector<std::string> imu_data_vector_buf;

unsigned char imu_data_buf[256];
unsigned char imu_data_buf[consts.READ_BUFFER_SIZE];
rt_usb_9axisimu::ImuData<double> imu_data;
std::string imu_data_oneline_buf;

Expand Down Expand Up @@ -207,15 +262,13 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void)
RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "")
{
serial_port_ = std::make_unique<rt_usb_9axisimu::SerialPort>(port.c_str());
has_completed_format_check_ = false;
data_format_ = DataFormat::NONE;
has_refreshed_imu_data_ = false;
}

RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::unique_ptr<rt_usb_9axisimu::SerialPort> serial_port)
{
serial_port_ = std::move(serial_port);
has_completed_format_check_ = false;
data_format_ = DataFormat::NONE;
has_refreshed_imu_data_ = false;
}
Expand Down Expand Up @@ -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<std::chrono::seconds>(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)
Expand Down
23 changes: 9 additions & 14 deletions src/rt_usb_9axisimu_driver_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<sensor_msgs::msg::Imu>("imu/data_raw", 1);
Expand Down
Loading

0 comments on commit b22d060

Please sign in to comment.