diff --git a/include/imu_node/imu_node.h b/include/imu_node/imu_node.h index 8e89d6f..a2b3f60 100644 --- a/include/imu_node/imu_node.h +++ b/include/imu_node/imu_node.h @@ -7,11 +7,12 @@ #pragma once -#include -#include #include -#include + #include +#include +#include +#include using namespace std::chrono_literals; @@ -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& acceleration, const std::vector& angular_velocity, - const std::vector& quaternion); - void processData(const uint8_t& raw_data); - bool checkSum(const std::vector::iterator& begin, const std::vector::iterator& end, - const uint8_t& check_data); - std::vector hexToShort(const std::vector& raw_data); - std::vector processAccelerationData(const std::vector& data); - std::vector processAngularVelocityData(const std::vector& data); - std::vector processQuaternionData(const std::vector& data); + void openSerial(const std::string & port, const int & baudrate, const float & time_out); + void publishMsg( + const std::vector & acceleration, const std::vector & angular_velocity, + const std::vector & quaternion); + void processData(const uint8_t & raw_data); + bool checkSum( + const std::vector::iterator & begin, const std::vector::iterator & end, + const uint8_t & check_data); + std::vector hexToShort(const std::vector & raw_data); + std::vector processAccelerationData(const std::vector & data); + std::vector processAngularVelocityData(const std::vector & data); + std::vector processQuaternionData(const std::vector & data); }; diff --git a/src/imu_node.cpp b/src/imu_node.cpp index aeaae27..4c8a676 100644 --- a/src/imu_node.cpp +++ b/src/imu_node.cpp @@ -26,15 +26,13 @@ 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); @@ -42,9 +40,7 @@ void ImuNode::openSerial(const std::string& port, const int& baudrate, const flo _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); } @@ -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 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; @@ -101,27 +89,21 @@ void ImuNode::processData(const uint8_t& raw_data) std::vector sliced_data(_buff.begin() + 2, _buff.begin() + _BUFFER_SIZE - 1); std::vector 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; @@ -132,58 +114,56 @@ void ImuNode::processData(const uint8_t& raw_data) return; } -std::vector ImuNode::hexToShort(const std::vector& raw_data) +std::vector ImuNode::hexToShort(const std::vector & raw_data) { // Convert a list of bytes to a list of shorts std::vector result; - for (std::vector::const_iterator it = raw_data.begin(); it != raw_data.end(); it += 2) - { + for (std::vector::const_iterator it = raw_data.begin(); it != raw_data.end(); it += 2) { int16_t value = (static_cast(*(it + 1)) << 8) | *it; result.push_back(value); } return result; } -bool ImuNode::checkSum(const std::vector::iterator& begin, const std::vector::iterator& end, - const uint8_t& check_data) +bool ImuNode::checkSum( + const std::vector::iterator & begin, const std::vector::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 ImuNode::processAccelerationData(const std::vector& data) +std::vector ImuNode::processAccelerationData(const std::vector & data) { std::vector 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 ImuNode::processAngularVelocityData(const std::vector& data) +std::vector ImuNode::processAngularVelocityData(const std::vector & data) { std::vector 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 ImuNode::processQuaternionData(const std::vector& data) +std::vector ImuNode::processQuaternionData(const std::vector & data) { std::vector 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& acceleration, const std::vector& angular_velocity, - const std::vector& quaternion) +void ImuNode::publishMsg( + const std::vector & acceleration, const std::vector & angular_velocity, + const std::vector & quaternion) { // Construct and publish the IMU message _imu_msg.header.stamp = this->get_clock()->now(); @@ -202,12 +182,11 @@ void ImuNode::publishMsg(const std::vector& 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(); - while (rclcpp::ok()) - { + while (rclcpp::ok()) { node->controlLoop(); } rclcpp::shutdown();