Skip to content
This repository has been archived by the owner on Aug 2, 2023. It is now read-only.

Commit

Permalink
apply format
Browse files Browse the repository at this point in the history
  • Loading branch information
@Alpaca-zip @github-actions
Alpaca-zip authored and github-actions[bot] committed Aug 2, 2023
1 parent 777f889 commit 923e199
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 66 deletions.
29 changes: 16 additions & 13 deletions include/imu_node/imu_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,12 @@

#pragma once

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <serial/serial.h>
#include <numeric>

#include <chrono>
#include <numeric>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>

using namespace std::chrono_literals;

Expand Down Expand Up @@ -43,14 +44,16 @@ class ImuNode : public rclcpp::Node
public:
ImuNode();
void controlLoop();
void openSerial(const std::string& port, const int& baudrate, const float& time_out);
void publishMsg(const std::vector<float>& acceleration, const std::vector<float>& angular_velocity,
const std::vector<float>& quaternion);
void processData(const uint8_t& raw_data);
bool checkSum(const std::vector<uint8_t>::iterator& begin, const std::vector<uint8_t>::iterator& end,
const uint8_t& check_data);
std::vector<int16_t> hexToShort(const std::vector<uint8_t>& raw_data);
std::vector<float> processAccelerationData(const std::vector<int16_t>& data);
std::vector<float> processAngularVelocityData(const std::vector<int16_t>& data);
std::vector<float> processQuaternionData(const std::vector<int16_t>& data);
void openSerial(const std::string & port, const int & baudrate, const float & time_out);
void publishMsg(
const std::vector<float> & acceleration, const std::vector<float> & angular_velocity,
const std::vector<float> & quaternion);
void processData(const uint8_t & raw_data);
bool checkSum(
const std::vector<uint8_t>::iterator & begin, const std::vector<uint8_t>::iterator & end,
const uint8_t & check_data);
std::vector<int16_t> hexToShort(const std::vector<uint8_t> & raw_data);
std::vector<float> processAccelerationData(const std::vector<int16_t> & data);
std::vector<float> processAngularVelocityData(const std::vector<int16_t> & data);
std::vector<float> processQuaternionData(const std::vector<int16_t> & data);
};
85 changes: 32 additions & 53 deletions src/imu_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,25 +26,21 @@ ImuNode::ImuNode() : rclcpp::Node("imu_node")
openSerial(_port, _baudrate, _time_out);
}

void ImuNode::openSerial(const std::string& port, const int& baudrate, const float& time_out)
void ImuNode::openSerial(const std::string & port, const int & baudrate, const float & time_out)
{
RCLCPP_INFO(this->get_logger(), "Port:%s baud:%d", port.c_str(), baudrate);
serial::Timeout to = serial::Timeout::simpleTimeout(time_out);

while (rclcpp::ok())
{
try
{
while (rclcpp::ok()) {
try {
// Attempt to open the serial port
_imu_serial.setPort(port);
_imu_serial.setBaudrate(baudrate);
_imu_serial.setTimeout(to);
_imu_serial.open();
RCLCPP_INFO(this->get_logger(), "\033[32mSerial port opened successfully!\033[0m");
break;
}
catch (serial::IOException& e)
{
} catch (serial::IOException & e) {
RCLCPP_WARN(this->get_logger(), "Serial port opening failure...Retrying");
rclcpp::sleep_for(5s);
}
Expand All @@ -53,45 +49,37 @@ void ImuNode::openSerial(const std::string& port, const int& baudrate, const flo

void ImuNode::controlLoop()
{
try
{
try {
// Check if there is any data waiting to be read from the serial port
if (_imu_serial.available())
{
if (_imu_serial.available()) {
// Read the waiting data and process it
std::vector<uint8_t> buff_data;
_imu_serial.read(buff_data, _imu_serial.available());
for (const auto& byte : buff_data)
{
for (const auto & byte : buff_data) {
processData(byte);
}
}
}
catch (const std::exception& e)
{
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "IMU disconnect...Retrying");
openSerial(_port, _baudrate, _time_out);
}
}

void ImuNode::processData(const uint8_t& raw_data)
void ImuNode::processData(const uint8_t & raw_data)
{
// Process each byte of raw data from the serial port
_buff.push_back(raw_data);

if (_buff[0] != _HEADER_BYTE)
{
if (_buff[0] != _HEADER_BYTE) {
_buff.clear();
return;
}

if (_buff.size() < _BUFFER_SIZE)
{
if (_buff.size() < _BUFFER_SIZE) {
return;
}

if (!checkSum(_buff.begin(), _buff.begin() + _BUFFER_SIZE - 1, _buff[_BUFFER_SIZE - 1]))
{
if (!checkSum(_buff.begin(), _buff.begin() + _BUFFER_SIZE - 1, _buff[_BUFFER_SIZE - 1])) {
RCLCPP_WARN(this->get_logger(), "Check failure for byte: %d", _buff[1]);
_buff.clear();
return;
Expand All @@ -101,27 +89,21 @@ void ImuNode::processData(const uint8_t& raw_data)
std::vector<uint8_t> sliced_data(_buff.begin() + 2, _buff.begin() + _BUFFER_SIZE - 1);
std::vector<int16_t> data = hexToShort(sliced_data);

if (_buff[1] == _ACCELERATION_BYTE)
{
if (_buff[1] == _ACCELERATION_BYTE) {
// Interpret acceleration data
_acceleration = processAccelerationData(data);
_acceleration_flag = true;
}
else if (_buff[1] == _ANGULAR_VELOCITY_BYTE)
{
} else if (_buff[1] == _ANGULAR_VELOCITY_BYTE) {
// Interpret angular velocity data
_angular_velocity = processAngularVelocityData(data);
_angular_velocity_flag = true;
}
else if (_buff[1] == _QUATERNION_BYTE)
{
} else if (_buff[1] == _QUATERNION_BYTE) {
// Interpret quaternion data
_quaternion = processQuaternionData(data);
_quaternion_flag = true;
}

if (_acceleration_flag && _angular_velocity_flag && _quaternion_flag)
{
if (_acceleration_flag && _angular_velocity_flag && _quaternion_flag) {
publishMsg(_acceleration, _angular_velocity, _quaternion);
_acceleration_flag = false;
_angular_velocity_flag = false;
Expand All @@ -132,58 +114,56 @@ void ImuNode::processData(const uint8_t& raw_data)
return;
}

std::vector<int16_t> ImuNode::hexToShort(const std::vector<uint8_t>& raw_data)
std::vector<int16_t> ImuNode::hexToShort(const std::vector<uint8_t> & raw_data)
{
// Convert a list of bytes to a list of shorts
std::vector<int16_t> result;
for (std::vector<uint8_t>::const_iterator it = raw_data.begin(); it != raw_data.end(); it += 2)
{
for (std::vector<uint8_t>::const_iterator it = raw_data.begin(); it != raw_data.end(); it += 2) {
int16_t value = (static_cast<int16_t>(*(it + 1)) << 8) | *it;
result.push_back(value);
}
return result;
}

bool ImuNode::checkSum(const std::vector<uint8_t>::iterator& begin, const std::vector<uint8_t>::iterator& end,
const uint8_t& check_data)
bool ImuNode::checkSum(
const std::vector<uint8_t>::iterator & begin, const std::vector<uint8_t>::iterator & end,
const uint8_t & check_data)
{
// Check the checksum of the data
uint8_t sum = std::accumulate(begin, end, 0);
return (sum & 0xFF) == check_data;
}

std::vector<float> ImuNode::processAccelerationData(const std::vector<int16_t>& data)
std::vector<float> ImuNode::processAccelerationData(const std::vector<int16_t> & data)
{
std::vector<float> acceleration(3);
for (size_t i = 0; i < 3; i++)
{
for (size_t i = 0; i < 3; i++) {
acceleration[i] = data[i] / 32768.0f * 4 * 9.80665;
}
return acceleration;
}

std::vector<float> ImuNode::processAngularVelocityData(const std::vector<int16_t>& data)
std::vector<float> ImuNode::processAngularVelocityData(const std::vector<int16_t> & data)
{
std::vector<float> angular_velocity(3);
for (size_t i = 0; i < 3; i++)
{
for (size_t i = 0; i < 3; i++) {
angular_velocity[i] = data[i] / 32768.0f * 2000 * M_PI / 180;
}
return angular_velocity;
}

std::vector<float> ImuNode::processQuaternionData(const std::vector<int16_t>& data)
std::vector<float> ImuNode::processQuaternionData(const std::vector<int16_t> & data)
{
std::vector<float> quaternion(4);
for (size_t i = 0; i < 4; i++)
{
for (size_t i = 0; i < 4; i++) {
quaternion[i] = data[i] / 32768.0f;
}
return quaternion;
}

void ImuNode::publishMsg(const std::vector<float>& acceleration, const std::vector<float>& angular_velocity,
const std::vector<float>& quaternion)
void ImuNode::publishMsg(
const std::vector<float> & acceleration, const std::vector<float> & angular_velocity,
const std::vector<float> & quaternion)
{
// Construct and publish the IMU message
_imu_msg.header.stamp = this->get_clock()->now();
Expand All @@ -202,12 +182,11 @@ void ImuNode::publishMsg(const std::vector<float>& acceleration, const std::vect
_imu_pub->publish(_imu_msg);
}

int main(int argc, char* argv[])
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ImuNode>();
while (rclcpp::ok())
{
while (rclcpp::ok()) {
node->controlLoop();
}
rclcpp::shutdown();
Expand Down

0 comments on commit 923e199

Please sign in to comment.