diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..21c3be6 --- /dev/null +++ b/.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 +... diff --git a/.github/workflows/formatter.yml b/.github/workflows/formatter.yml new file mode 100644 index 0000000..2cb27d3 --- /dev/null +++ b/.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 diff --git a/.github/workflows/ros2-build-check-bot.yml b/.github/workflows/ros2-build-check-bot.yml index f489279..ee5fe8c 100644 --- a/.github/workflows/ros2-build-check-bot.yml +++ b/.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 diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..cb9fb9d --- /dev/null +++ b/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. diff --git a/firmware/firmware.ino b/firmware/firmware.ino index f434371..944d37e 100644 --- a/firmware/firmware.ino +++ b/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 diff --git a/include/imu_node/imu_node.h b/include/imu_node/imu_node.h index 4ccc6b6..a2b3f60 100644 --- a/include/imu_node/imu_node.h +++ b/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 -#include #include -#include + #include +#include +#include +#include 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& 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 897cea1..4c8a676 100644 --- a/src/imu_node.cpp +++ b/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,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); @@ -35,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); } @@ -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 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 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; @@ -125,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(); @@ -195,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();