Merge pull request #8 from Alpaca-zip/feature/license_and_formatter

Feature/license and formatter

.clang-format

@@ -0,0 +1,19 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+ AfterClass: true
+ AfterFunction: true
+ AfterNamespace: true
+ AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false
+...

.github/workflows/formatter.yml

@@ -0,0 +1,23 @@
+name: Formatter
+
+on:
+ pull_request:
+
+jobs:
+ formatter:
+ runs-on: ubuntu-latest
+ steps:
+ - name: checkout
+ uses: actions/checkout@v2
+ with:
+ ref: ${{ github.head_ref }}
+ - name: install dependencies
+ run: |
+ sudo apt-get update
+ sudo apt-get install -y clang-format
+ - name: clang-format
+ run: find . -type f \( -name "*.cpp" -or -name "*.hpp" -or -name "*.h" \) -exec clang-format -i -style=file {} \;
+ - name: auto commit
+ uses: stefanzweifel/git-auto-commit-action@v4
+ with:
+ commit_message: apply format

.github/workflows/ros2-build-check-bot.yml

@@ -7,15 +7,15 @@ jobs:
  build:
  runs-on: ubuntu-22.04
  steps:
- - name: Checkout
+ - name: checkout
  uses: actions/checkout@v2
  with:
  ref: ${{ github.head_ref }}
- - name: Setup ROS2
+ - name: setup ROS2
  uses: ros-tooling/setup-ros@v0.6
  with:
  required-ros-distributions: humble
- - name: Build
+ - name: build
  uses: ros-tooling/action-ros-ci@v0.3
  with:
  package-name: icm_20948 serial

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 Alpaca-zip
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

firmware/firmware.ino

@@ -1,15 +1,21 @@
-/****************************************************************
- * This script is based on Example10_DMP_FastMultipleSensors.ino in the ICM 20948 Arduino Library.
- * 
- * ** Important note: by default the DMP functionality is disabled in the library
- * ** as the DMP firmware takes up 14301 Bytes of program memory.
- * ** To use the DMP, you will need to:
- * ** Edit ICM_20948_C.h
- * ** Uncomment line 29: #define ICM_20948_USE_DMP
- * ** Save changes
- * ** If you are using Windows, you can find ICM_20948_C.h in:
- * ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
- ***************************************************************/
+/*
+ This script is based on Example10_DMP_FastMultipleSensors.ino in the ICM 20948 Arduino Library.
+ https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary
+
+ Copyright 2023 Alapaca-zip
+
+ This software is released under the MIT License.
+ http://opensource.org/licenses/mit-license.php
+
+ ** Important note: by default the DMP functionality is disabled in the library
+ ** as the DMP firmware takes up 14301 Bytes of program memory.
+ ** To use the DMP, you will need to:
+ ** Edit ICM_20948_C.h
+ ** Uncomment line 29: #define ICM_20948_USE_DMP
+ ** Save changes
+ ** If you are using Windows, you can find ICM_20948_C.h in:
+ ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
+*/
 
 #define USE_USBCON // seeeduino xiao needs this flag to be defined
 #define SERIAL_PORT Serial

include/imu_node/imu_node.h

@@ -1,10 +1,18 @@
+/*
+ Copyright 2023 Alapaca-zip
+
+ This software is released under the MIT License.
+ http://opensource.org/licenses/mit-license.php
+*/
+
 #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;
 
@@ -36,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);
 };

src/imu_node.cpp

@@ -1,3 +1,10 @@
+/*
+ Copyright 2023 Alapaca-zip
+
+ This software is released under the MIT License.
+ http://opensource.org/licenses/mit-license.php
+*/
+
 #include "imu_node/imu_node.h"
 
 ImuNode::ImuNode() : rclcpp::Node("imu_node")
@@ -19,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);
  }
@@ -46,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;
@@ -94,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;
@@ -125,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();
@@ -195,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();