Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

checkDataFormat() and unit test updates #54

Merged
merged 13 commits into from
Aug 6, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/industrial_ci.yml
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
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
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
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
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