From e859ecd0895bfdceae0c72a22a45ac6aaeae795f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christopher=20Str=C3=B8m?= Date: Sat, 22 Jun 2024 14:48:20 +0000 Subject: [PATCH] Initial ROS2 port and refactor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Christopher Strøm --- CMakeLists.txt | 66 ++-- .../ros_stim300_driver/ros_stim300_driver.hpp | 66 ++++ .../stim300_driver/datagram_parser.hpp | 3 +- .../driver_stim300.hpp} | 6 +- .../stim300_driver/serial_driver.hpp | 0 .../stim300_driver/serial_unix.hpp | 4 +- .../stim300_driver/stim300_constants.hpp | 0 launch/stim300_driver.launch | 7 - launch/stim300_driver.launch.py | 41 ++ package.xml | 22 +- src/CMakeLists.txt | 3 + src/ros_stim300_driver/CMakeLists.txt | 63 ++++ src/ros_stim300_driver/ros_stim300_driver.cpp | 292 +++++++++++++++ .../ros_stim300_driver_node.cpp | 10 + src/stim300_driver/CMakeLists.txt | 26 ++ src/{ => stim300_driver}/datagram_parser.cpp | 3 +- src/{ => stim300_driver}/driver_stim300.cpp | 2 +- src/{ => stim300_driver}/serial_unix.cpp | 2 +- src/stim300_driver_node.cpp | 349 ------------------ test/CMakeLists.txt | 38 ++ test/check_driver_stim300.cpp | 2 +- test/check_stim300_constants.cpp | 2 +- test/mock_serial_driver.h | 4 +- 23 files changed, 579 insertions(+), 432 deletions(-) create mode 100644 include/ros_stim300_driver/ros_stim300_driver.hpp rename src/datagram_parser.h => include/stim300_driver/datagram_parser.hpp (98%) rename include/{driver_stim300.h => stim300_driver/driver_stim300.hpp} (96%) rename src/serial_driver.h => include/stim300_driver/serial_driver.hpp (100%) rename src/serial_unix.h => include/stim300_driver/serial_unix.hpp (93%) rename src/stim300_constants.h => include/stim300_driver/stim300_constants.hpp (100%) delete mode 100644 launch/stim300_driver.launch create mode 100644 launch/stim300_driver.launch.py create mode 100644 src/CMakeLists.txt create mode 100644 src/ros_stim300_driver/CMakeLists.txt create mode 100644 src/ros_stim300_driver/ros_stim300_driver.cpp create mode 100644 src/ros_stim300_driver/ros_stim300_driver_node.cpp create mode 100644 src/stim300_driver/CMakeLists.txt rename src/{ => stim300_driver}/datagram_parser.cpp (99%) rename src/{ => stim300_driver}/driver_stim300.cpp (99%) rename src/{ => stim300_driver}/serial_unix.cpp (98%) delete mode 100644 src/stim300_driver_node.cpp create mode 100644 test/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index b4967ee..ab1d583 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,53 +1,33 @@ -cmake_minimum_required(VERSION 3.5.1) +cmake_minimum_required(VERSION 3.5) project(driver_stim300) -set(CMAKE_CXX_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -add_library(stim300_driver_lib - src/driver_stim300.cpp - src/datagram_parser.cpp - src/serial_unix.cpp) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) -target_include_directories(stim300_driver_lib - PUBLIC - $ - $ - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src - ) -target_link_libraries(stim300_driver_lib) +add_subdirectory(src) -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - std_srvs - ) -catkin_package( - CATKIN_DEPENDS sensor_msgs std_srvs +install(TARGETS + stim300_driver_lib + ros_stim300_driver_node + DESTINATION lib/${PROJECT_NAME} ) -add_executable(stim300_driver_node src/stim300_driver_node.cpp) -target_include_directories(stim300_driver_node - PRIVATE - ${catkin_INCLUDE_DIRS}) -target_link_libraries(stim300_driver_node PRIVATE stim300_driver_lib ${catkin_LIBRARIES}) - - -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(check_datasheet_constanst - test/check_stim300_constants.cpp - src/stim300_constants.h) - - target_link_libraries(check_datasheet_constanst ${catkin_LIBRARIES}) -endif() - -if (CATKIN_ENABLE_TESTING) - catkin_add_gmock(check_driver_stim300 - test/check_driver_stim300.cpp - test/mock_serial_driver.h - src/serial_driver.h) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) - target_link_libraries(check_driver_stim300 stim300_driver_lib ${catkin_LIBRARIES}) +if(BUILD_TESTING) + add_subdirectory(test) endif() - +ament_package() diff --git a/include/ros_stim300_driver/ros_stim300_driver.hpp b/include/ros_stim300_driver/ros_stim300_driver.hpp new file mode 100644 index 0000000..07330f4 --- /dev/null +++ b/include/ros_stim300_driver/ros_stim300_driver.hpp @@ -0,0 +1,66 @@ +#ifndef ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP +#define ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP + +#include "stim300_driver/driver_stim300.hpp" +#include "stim300_driver/serial_unix.hpp" +#include "math.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "iostream" +#include + +constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; +constexpr double ACC_TOLERANCE{0.1}; +constexpr double MAX_DROPPED_ACC_X_MSG{5}; +constexpr double MAX_DROPPED_ACC_Y_MSG{5}; +constexpr double MAX_DROPPED_ACC_Z_MSG{5}; +constexpr double MAX_DROPPED_GYRO_X_MSG{5}; +constexpr double MAX_DROPPED_GYRO_Y_MSG{5}; +constexpr double MAX_DROPPED_GYRO_Z_MSG{5}; +constexpr double GYRO_X_PEAK_TO_PEAK_NOISE{0.0002}; +constexpr double GYRO_Y_PEAK_TO_PEAK_NOISE{0.0002}; +constexpr double GYRO_Z_PEAK_TO_PEAK_NOISE{0.0002}; + +struct Quaternion +{ + double w, x, y, z; +}; + +struct EulerAngles { + double roll, pitch, yaw; +}; + +Quaternion FromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X) + + +class Stim300DriverNode : public rclcpp::Node +{ +public: + Stim300DriverNode(); + +private: + void timerCallback(); + + bool responseCalibrateIMU(const std::shared_ptr request, + std::shared_ptr response); + + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Service::SharedPtr calibration_service_; + rclcpp::TimerBase::SharedPtr timer_; + + bool calibration_mode_ = false; + + std::string device_name_; + double variance_gyro_; + double variance_acc_; + double gravity_; + double sample_rate_; + + sensor_msgs::msg::Imu stim300msg_; + + std::unique_ptr serial_driver_; + std::unique_ptr driver_stim300_; +}; + +#endif // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP \ No newline at end of file diff --git a/src/datagram_parser.h b/include/stim300_driver/datagram_parser.hpp similarity index 98% rename from src/datagram_parser.h rename to include/stim300_driver/datagram_parser.hpp index 2531c91..8ae861a 100644 --- a/src/datagram_parser.h +++ b/include/stim300_driver/datagram_parser.hpp @@ -2,12 +2,13 @@ #ifndef DRIVER_STIM300_DATAGRAM_PARSER_H #define DRIVER_STIM300_DATAGRAM_PARSER_H -#include "stim300_constants.h" +#include "stim300_constants.hpp" #include #include #include #include #include + using namespace stim_const; namespace stim_300 { diff --git a/include/driver_stim300.h b/include/stim300_driver/driver_stim300.hpp similarity index 96% rename from include/driver_stim300.h rename to include/stim300_driver/driver_stim300.hpp index 04d513f..ce96244 100644 --- a/include/driver_stim300.h +++ b/include/stim300_driver/driver_stim300.hpp @@ -2,9 +2,9 @@ #ifndef DRIVER_STIM300_DRIVER_STIM300_H #define DRIVER_STIM300_DRIVER_STIM300_H -#include "../src/datagram_parser.h" -#include "../src/serial_unix.h" -#include "../src/stim300_constants.h" +#include "datagram_parser.hpp" +#include "serial_unix.hpp" +#include "stim300_constants.hpp" #include #include diff --git a/src/serial_driver.h b/include/stim300_driver/serial_driver.hpp similarity index 100% rename from src/serial_driver.h rename to include/stim300_driver/serial_driver.hpp diff --git a/src/serial_unix.h b/include/stim300_driver/serial_unix.hpp similarity index 93% rename from src/serial_unix.h rename to include/stim300_driver/serial_unix.hpp index f22d656..320be01 100644 --- a/src/serial_unix.h +++ b/include/stim300_driver/serial_unix.hpp @@ -1,13 +1,13 @@ #ifndef DRIVER_STIM300_SERIAL_UBUNTU_H #define DRIVER_STIM300_SERIAL_UBUNTU_H -#include "serial_driver.h" +#include "serial_driver.hpp" #include #include #include #include -#include "stim300_constants.h" +#include "stim300_constants.hpp" #include #include #include diff --git a/src/stim300_constants.h b/include/stim300_driver/stim300_constants.hpp similarity index 100% rename from src/stim300_constants.h rename to include/stim300_driver/stim300_constants.hpp diff --git a/launch/stim300_driver.launch b/launch/stim300_driver.launch deleted file mode 100644 index 21af3c0..0000000 --- a/launch/stim300_driver.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/launch/stim300_driver.launch.py b/launch/stim300_driver.launch.py new file mode 100644 index 0000000..3cf3a66 --- /dev/null +++ b/launch/stim300_driver.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'device_name', + default_value='/dev/ttyUSB0', + description='Device name' + ), + DeclareLaunchArgument( + 'standard_deviation_of_gyro', + default_value='0.0004', + description='Standard deviation of gyro' + ), + DeclareLaunchArgument( + 'standard_deviation_of_acc', + default_value='0.004', + description='Standard deviation of accelerometer' + ), + DeclareLaunchArgument( + 'sample_rate', + default_value='125', + description='Sample rate' + ), + Node( + package='driver_stim300', + executable='stim300_driver_node', + name='stim300driver', + parameters=[{ + 'device_name': LaunchConfiguration('device_name'), + 'standard_deviation_of_gyro': LaunchConfiguration('standard_deviation_of_gyro'), + 'standard_deviation_of_acc': LaunchConfiguration('standard_deviation_of_acc'), + 'sample_rate': LaunchConfiguration('sample_rate') + }] + ) + ]) + diff --git a/package.xml b/package.xml index 5d256ea..f3421d5 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + driver_stim300 0.0.0 The drivers-imu_stim300 package @@ -9,18 +10,7 @@ MIT - - - - - - - - - - - - catkin + colcon roscpp sensor_msgs std_srvs @@ -31,10 +21,4 @@ sensor_msgs std_srvs - - - - - - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..3bc4de1 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(stim300_driver) +add_subdirectory(ros_stim300_driver) + diff --git a/src/ros_stim300_driver/CMakeLists.txt b/src/ros_stim300_driver/CMakeLists.txt new file mode 100644 index 0000000..18d9460 --- /dev/null +++ b/src/ros_stim300_driver/CMakeLists.txt @@ -0,0 +1,63 @@ +set(ROS_DRIVER_LIB ros_stim300_driver_lib) + +add_library( + ${ROS_DRIVER_LIB} + SHARED +) + +target_sources( + ${ROS_DRIVER_LIB} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/ros_stim300_driver.cpp +) + +ament_target_dependencies( + ${ROS_DRIVER_LIB} + PUBLIC + rclcpp + sensor_msgs + std_srvs +) + + +target_include_directories( + ${ROS_DRIVER_LIB} + PUBLIC + $ +) + +target_link_libraries( + ${ROS_DRIVER_LIB} + PUBLIC + stim300_driver_lib +) + +install(TARGETS + ${ROS_DRIVER_LIB} + DESTINATION lib +) + + +set(STIM300_DRIVER_BINARY_NAME ros_stim300_driver_node) + +add_executable( + ${STIM300_DRIVER_BINARY_NAME} + ${CMAKE_CURRENT_SOURCE_DIR}/ros_stim300_driver_node.cpp +) + +target_include_directories( + ${STIM300_DRIVER_BINARY_NAME} + PUBLIC + $ +) + +target_link_libraries( + ${STIM300_DRIVER_BINARY_NAME} + ${ROS_DRIVER_LIB} +) + +install( + TARGETS + ${STIM300_DRIVER_BINARY_NAME} + DESTINATION lib/${PROJECT_NAME} +) diff --git a/src/ros_stim300_driver/ros_stim300_driver.cpp b/src/ros_stim300_driver/ros_stim300_driver.cpp new file mode 100644 index 0000000..85a4f67 --- /dev/null +++ b/src/ros_stim300_driver/ros_stim300_driver.cpp @@ -0,0 +1,292 @@ +#include "ros_stim300_driver/ros_stim300_driver.hpp" + + +Quaternion FromRPYToQuaternion(EulerAngles angles) +{ + double cy = cos(angles.yaw * 0.5); + double sy = sin(angles.yaw * 0.5); + double cp = cos(angles.pitch * 0.5); + double sp = sin(angles.pitch * 0.5); + double cr = cos(angles.roll * 0.5); + double sr = sin(angles.roll * 0.5); + + Quaternion q; + q.w = cy * cp * cr + sy * sp * sr; + q.x = cy * cp * sr - sy * sp * cr; + q.y = sy * cp * sr + cy * sp * cr; + q.z = sy * cp * cr - cy * sp * sr; + + return q; +} + +Stim300DriverNode::Stim300DriverNode() : Node("stim300_driver_node"), gravity_(9.80665), sample_rate_(125) +{ + this->declare_parameter("device_name", "/dev/ttyUSB0"); + this->declare_parameter("variance_gyro", 0.0001 * 2 * 4.6 * pow(10, -4)); + this->declare_parameter("variance_acc", 0.000055); + this->declare_parameter("sample_rate", 125.0); + this->declare_parameter("gravity", 9.80665); + + this->get_parameter("device_name", device_name_); + this->get_parameter("variance_gyro", variance_gyro_); + this->get_parameter("variance_acc", variance_acc_); + this->get_parameter("sample_rate", sample_rate_); + this->get_parameter("gravity", gravity_); + + imu_publisher_ = this->create_publisher("imu/data_raw", 1000); + calibration_service_ = this->create_service( + "IMU_calibration", std::bind(&Stim300DriverNode::responseCalibrateIMU, this, std::placeholders::_1, std::placeholders::_2)); + + stim300msg_.angular_velocity_covariance[0] = 0.0000027474; + stim300msg_.angular_velocity_covariance[4] = 0.0000027474; + stim300msg_.angular_velocity_covariance[8] = 0.000007312; + stim300msg_.linear_acceleration_covariance[0] = 0.00041915; + stim300msg_.linear_acceleration_covariance[4] = 0.00041915; + stim300msg_.linear_acceleration_covariance[8] = 0.000018995; + stim300msg_.orientation.x = 0.00000024358; + stim300msg_.orientation.y = 0.00000024358; + stim300msg_.orientation.z = 0.00000024358; + stim300msg_.header.frame_id = "imu_0"; + + try + { + serial_driver_ = std::make_unique(device_name_, stim_const::BaudRate::BAUD_921600); + driver_stim300_ = std::make_unique(*serial_driver_); + RCLCPP_INFO(this->get_logger(), "STIM300 IMU driver initialized successfully"); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000.0 / (sample_rate_ * 2))), + std::bind(&Stim300DriverNode::timerCallback, this)); + } + catch (std::runtime_error &error) + { + RCLCPP_ERROR(this->get_logger(), "%s", error.what()); + } +} + +void Stim300DriverNode::timerCallback() +{ + int difference_in_dataGram{0}; + int count_messages{0}; + int number_of_samples{0}; + double inclination_x{0}; + double inclination_y{0}; + double inclination_z{0}; + + double average_calibration_roll{0}; + double average_calibration_pitch{0}; + double inclination_x_calibration_sum{0}; + double inclination_y_calibration_sum{0}; + double inclination_z_calibration_sum{0}; + double inclination_x_average{0}; + double inclination_y_average{0}; + double inclination_z_average{0}; + + // Acc wild point filter + std::vector acceleration_buffer_x{}; + std::vector acceleration_buffer_y{}; + std::vector acceleration_buffer_z{}; + double dropped_acceleration_x_msg{0.0}; + double dropped_acceleration_y_msg{0.0}; + double dropped_acceleration_z_msg{0.0}; + + // Gyro wild point filter + std::vector gyro_buffer_x{}; + std::vector gyro_buffer_y{}; + std::vector gyro_buffer_z{}; + double dropped_gyro_x_msg{0.0}; + double dropped_gyro_y_msg{0.0}; + double dropped_gyro_z_msg{0.0}; + + switch (driver_stim300_->update()) + { + case Stim300Status::NORMAL: + break; + case Stim300Status::OUTSIDE_OPERATING_CONDITIONS: + RCLCPP_DEBUG(this->get_logger(), "Stim 300 outside operating conditions"); + break; + case Stim300Status::NEW_MEASURMENT: + inclination_x = driver_stim300_->getIncX(); + inclination_y = driver_stim300_->getIncY(); + inclination_z = driver_stim300_->getIncZ(); + Quaternion q; + EulerAngles RPY; + if (calibration_mode_ == true) + { + if (number_of_samples < NUMBER_OF_CALIBRATION_SAMPLES) + { + number_of_samples++; + inclination_x_calibration_sum += inclination_x; + inclination_y_calibration_sum += inclination_y; + inclination_z_calibration_sum += inclination_z; + } + else + { + inclination_x_average = inclination_x_calibration_sum / NUMBER_OF_CALIBRATION_SAMPLES; + inclination_y_average = inclination_y_calibration_sum / NUMBER_OF_CALIBRATION_SAMPLES; + inclination_z_average = inclination_z_calibration_sum / NUMBER_OF_CALIBRATION_SAMPLES; + + average_calibration_roll = atan2(inclination_y_average, inclination_z_average); + average_calibration_pitch = atan2(-inclination_x_average, sqrt(pow(inclination_y_average, 2) + pow(inclination_z_average, 2))); + std::cout << average_calibration_roll << std::endl; + std::cout << average_calibration_pitch << std::endl; + RCLCPP_INFO(this->get_logger(), "roll: %f", average_calibration_roll); + RCLCPP_INFO(this->get_logger(), "pitch: %f", average_calibration_pitch); + RCLCPP_INFO(this->get_logger(), "IMU Calibrated"); + calibration_mode_ = false; + } + break; + } + else + { + RPY.roll = atan2(inclination_y, inclination_z); + RPY.pitch = atan2(-inclination_x, sqrt(pow(inclination_y, 2) + pow(inclination_z, 2))); + q = FromRPYToQuaternion(RPY); + + // Acceleration wild point filter + acceleration_buffer_x.push_back(driver_stim300_->getAccX() * gravity_); + acceleration_buffer_y.push_back(driver_stim300_->getAccY() * gravity_); + acceleration_buffer_z.push_back(driver_stim300_->getAccZ() * gravity_); + stim300msg_.header.stamp = this->now(); + + if (acceleration_buffer_x.size() == 2 && acceleration_buffer_y.size() == 2 && acceleration_buffer_z.size() == 2) + { + if (std::abs(acceleration_buffer_x.back() - acceleration_buffer_x.front()) < ACC_TOLERANCE || dropped_acceleration_x_msg > MAX_DROPPED_ACC_X_MSG) + { + stim300msg_.linear_acceleration.x = acceleration_buffer_x.back(); + dropped_acceleration_x_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "ACC_X_MSG wild point rejected"); + dropped_acceleration_x_msg += 1; + } + + if (std::abs(acceleration_buffer_y.back() - acceleration_buffer_y.front()) < ACC_TOLERANCE || dropped_acceleration_y_msg > MAX_DROPPED_ACC_Y_MSG) + { + stim300msg_.linear_acceleration.y = acceleration_buffer_y.back(); + dropped_acceleration_y_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "ACC_Y_MSG wild point rejected"); + dropped_acceleration_y_msg += 1; + } + + if (std::abs(acceleration_buffer_z.back() - acceleration_buffer_z.front()) < ACC_TOLERANCE || dropped_acceleration_z_msg > MAX_DROPPED_ACC_Z_MSG) + { + stim300msg_.linear_acceleration.z = acceleration_buffer_z.back(); + dropped_acceleration_z_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "ACC_Z_MSG wild point rejected"); + dropped_acceleration_z_msg += 1; + } + // Empty acceleration buffers + acceleration_buffer_x = std::vector{acceleration_buffer_x.back()}; + acceleration_buffer_y = std::vector{acceleration_buffer_y.back()}; + acceleration_buffer_z = std::vector{acceleration_buffer_z.back()}; + } + else + { + stim300msg_.linear_acceleration.x = driver_stim300_->getAccX() * gravity_; + stim300msg_.linear_acceleration.y = driver_stim300_->getAccY() * gravity_; + stim300msg_.linear_acceleration.z = driver_stim300_->getAccZ() * gravity_; + } + + // Gyro wild point filter + gyro_buffer_x.push_back(driver_stim300_->getGyroX()); + gyro_buffer_y.push_back(driver_stim300_->getGyroY()); + gyro_buffer_z.push_back(driver_stim300_->getGyroZ()); + + if (gyro_buffer_x.size() == 2 && gyro_buffer_y.size() == 2 && gyro_buffer_z.size() == 2) + { + if (std::abs(gyro_buffer_x.back() - gyro_buffer_x.front()) < std::max(2 * std::abs(gyro_buffer_x.front()), GYRO_X_PEAK_TO_PEAK_NOISE) || dropped_gyro_x_msg > MAX_DROPPED_GYRO_X_MSG) + { + stim300msg_.angular_velocity.x = gyro_buffer_x.back(); + dropped_gyro_x_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "GYRO_X_MSG wild point rejected"); + dropped_gyro_x_msg += 1; + } + + if (std::abs(gyro_buffer_y.back() - gyro_buffer_y.front()) < std::max(2 * std::abs(gyro_buffer_y.front()), GYRO_Y_PEAK_TO_PEAK_NOISE) || dropped_gyro_y_msg > MAX_DROPPED_GYRO_Y_MSG) + { + stim300msg_.angular_velocity.y = gyro_buffer_y.back(); + dropped_gyro_y_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "GYRO_Y_MSG wild point rejected"); + dropped_gyro_y_msg += 1; + } + + if (std::abs(gyro_buffer_z.back() - gyro_buffer_z.front()) < std::max(2 * std::abs(gyro_buffer_z.front()), GYRO_Z_PEAK_TO_PEAK_NOISE) || dropped_gyro_z_msg > MAX_DROPPED_GYRO_Z_MSG) + { + stim300msg_.angular_velocity.z = gyro_buffer_z.back(); + dropped_gyro_z_msg = 0; + } + else + { + RCLCPP_DEBUG(this->get_logger(), "GYRO_Z_MSG wild point rejected"); + dropped_gyro_z_msg += 1; + } + + // Empty buffers + gyro_buffer_x = std::vector{gyro_buffer_x.back()}; + gyro_buffer_y = std::vector{gyro_buffer_y.back()}; + gyro_buffer_z = std::vector{gyro_buffer_z.back()}; + } + else + { + stim300msg_.angular_velocity.x = driver_stim300_->getGyroX(); + stim300msg_.angular_velocity.y = driver_stim300_->getGyroY(); + stim300msg_.angular_velocity.z = driver_stim300_->getGyroZ(); + } + + stim300msg_.orientation.w = q.w; + stim300msg_.orientation.x = q.x; + stim300msg_.orientation.y = q.y; + stim300msg_.orientation.z = q.z; + imu_publisher_->publish(stim300msg_); + break; + } + case Stim300Status::CONFIG_CHANGED: + RCLCPP_INFO(this->get_logger(), "Updated Stim 300 imu config: "); + RCLCPP_INFO(this->get_logger(), "%s", driver_stim300_->printSensorConfig().c_str()); + sample_rate_ = driver_stim300_->getSampleRate() * 2; + timer_->cancel(); + timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000.0 / (sample_rate_))), + std::bind(&Stim300DriverNode::timerCallback, this)); + break; + case Stim300Status::STARTING_SENSOR: + RCLCPP_INFO(this->get_logger(), "Stim 300 IMU is warming up."); + break; + case Stim300Status::SYSTEM_INTEGRITY_ERROR: + RCLCPP_WARN(this->get_logger(), "Stim 300 IMU system integrity error."); + break; + case Stim300Status::OVERLOAD: + RCLCPP_WARN(this->get_logger(), "Stim 300 IMU overload."); + break; + case Stim300Status::ERROR_IN_MEASUREMENT_CHANNEL: + RCLCPP_WARN(this->get_logger(), "Stim 300 IMU error in measurement channel."); + break; + case Stim300Status::ERROR: + RCLCPP_WARN(this->get_logger(), "Stim 300 IMU: internal error."); + } +} + +bool Stim300DriverNode::responseCalibrateIMU(const std::shared_ptr request, + std::shared_ptr response) +{ + if (!calibration_mode_) + { + calibration_mode_ = true; + response->message = "IMU in calibration mode"; + response->success = true; + } + + return true; +} diff --git a/src/ros_stim300_driver/ros_stim300_driver_node.cpp b/src/ros_stim300_driver/ros_stim300_driver_node.cpp new file mode 100644 index 0000000..b161088 --- /dev/null +++ b/src/ros_stim300_driver/ros_stim300_driver_node.cpp @@ -0,0 +1,10 @@ +#include "rclcpp/rclcpp.hpp" +#include "ros_stim300_driver/ros_stim300_driver.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/stim300_driver/CMakeLists.txt b/src/stim300_driver/CMakeLists.txt new file mode 100644 index 0000000..f528afe --- /dev/null +++ b/src/stim300_driver/CMakeLists.txt @@ -0,0 +1,26 @@ +set(DRIVER_LIB stim300_driver_lib) + +add_library(${DRIVER_LIB} + SHARED +) + +target_sources( + ${DRIVER_LIB} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/datagram_parser.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/serial_unix.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/driver_stim300.cpp +) + +target_include_directories( + ${DRIVER_LIB} + PUBLIC + $ + $ +) + +install( + TARGETS ${DRIVER_LIB} + DESTINATION lib +) + diff --git a/src/datagram_parser.cpp b/src/stim300_driver/datagram_parser.cpp similarity index 99% rename from src/datagram_parser.cpp rename to src/stim300_driver/datagram_parser.cpp index c593d03..9f43f01 100644 --- a/src/datagram_parser.cpp +++ b/src/stim300_driver/datagram_parser.cpp @@ -1,5 +1,4 @@ - -#include "datagram_parser.h" +#include "stim300_driver/datagram_parser.hpp" using namespace stim_const; using namespace stim_300; diff --git a/src/driver_stim300.cpp b/src/stim300_driver/driver_stim300.cpp similarity index 99% rename from src/driver_stim300.cpp rename to src/stim300_driver/driver_stim300.cpp index 96dc2d8..5b7010a 100644 --- a/src/driver_stim300.cpp +++ b/src/stim300_driver/driver_stim300.cpp @@ -1,5 +1,5 @@ -#include "driver_stim300.h" +#include "stim300_driver/driver_stim300.hpp" #include DriverStim300::DriverStim300(SerialDriver &serial_driver, diff --git a/src/serial_unix.cpp b/src/stim300_driver/serial_unix.cpp similarity index 98% rename from src/serial_unix.cpp rename to src/stim300_driver/serial_unix.cpp index c450976..553c10b 100644 --- a/src/serial_unix.cpp +++ b/src/stim300_driver/serial_unix.cpp @@ -1,5 +1,5 @@ -#include "serial_unix.h" +#include "stim300_driver/serial_unix.hpp" #include // Everything is learned from // https://en.wikibooks.org/wiki/Serial_Programming/termios diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp deleted file mode 100644 index 19e472f..0000000 --- a/src/stim300_driver_node.cpp +++ /dev/null @@ -1,349 +0,0 @@ -#include "driver_stim300.h" -#include "serial_unix.h" -#include "math.h" -#include "ros/ros.h" -#include "sensor_msgs/Imu.h" -#include "std_srvs/Trigger.h" -#include "std_srvs/Empty.h" -#include "iostream" - -bool calibration_mode{false}; -constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; -constexpr double ACC_TOLERANCE{0.1}; -constexpr double MAX_DROPPED_ACC_X_MSG{5}; -constexpr double MAX_DROPPED_ACC_Y_MSG{5}; -constexpr double MAX_DROPPED_ACC_Z_MSG{5}; -constexpr double MAX_DROPPED_GYRO_X_MSG{5}; -constexpr double MAX_DROPPED_GYRO_Y_MSG{5}; -constexpr double MAX_DROPPED_GYRO_Z_MSG{5}; -constexpr double GYRO_X_PEAK_TO_PEAK_NOISE{0.0002}; -constexpr double GYRO_Y_PEAK_TO_PEAK_NOISE{0.0002}; -constexpr double GYRO_Z_PEAK_TO_PEAK_NOISE{0.0002}; - -struct Quaternion -{ - double w, x, y, z; -}; - -struct EulerAngles { - double roll, pitch, yaw; -}; - -Quaternion FromRPYToQuaternion(EulerAngles angles) // yaw (Z), pitch (Y), roll (X) -{ - // Abbreviations for the various angular functions - double cy = cos(angles.yaw * 0.5); - double sy = sin(angles.yaw * 0.5); - double cp = cos(angles.pitch * 0.5); - double sp = sin(angles.pitch * 0.5); - double cr = cos(angles.roll * 0.5); - double sr = sin(angles.roll * 0.5); - - Quaternion q; - q.w = cy * cp * cr + sy * sp * sr; - q.x = cy * cp * sr - sy * sp * cr; - q.y = sy * cp * sr + cy * sp * cr; - q.z = sy * cp * cr - cy * sp * sr; - - return q; -} - - -bool responseCalibrateIMU(std_srvs::Trigger::Request &calibration_request, std_srvs::Trigger::Response &calibration_response) -{ - - if (calibration_mode == false) - { - calibration_mode = true; - calibration_response.message = "IMU in calibration mode "; - calibration_response.success = true; - } - - return true; -} - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "stim300_driver_node"); - - ros::NodeHandle node; - - std::string device_name; - double variance_gyro{0}; - double variance_acc{0}; - int sample_rate{0}; - double gravity{0}; - - - node.param("device_name", device_name, "/dev/ttyUSB0"); - - node.param("variance_gyro", variance_gyro,0.0001*2*4.6*pow(10,-4)); - node.param("variance_acc", variance_acc, 0.000055); - node.param("sample_rate", sample_rate, 125); - - node.param("gravity", gravity, 9.80665); - - // These values have been estimated by having beluga in a pool for a couple of minutes, and then calculate the variance for each values - sensor_msgs::Imu stim300msg{}; - stim300msg.angular_velocity_covariance[0] = 0.0000027474; - stim300msg.angular_velocity_covariance[4] = 0.0000027474; - stim300msg.angular_velocity_covariance[8] = 0.000007312; - stim300msg.linear_acceleration_covariance[0] = 0.00041915; - stim300msg.linear_acceleration_covariance[4] = 0.00041915; - stim300msg.linear_acceleration_covariance[8] = 0.000018995; - stim300msg.orientation.x = 0.00000024358; - stim300msg.orientation.y = 0.00000024358; - stim300msg.orientation.z = 0.00000024358; - stim300msg.header.frame_id = "imu_0"; - - - ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000); - //ros::Publisher orientationPublisher = node.advertise("imu/orientation", 1000); - ros::ServiceServer service = node.advertiseService("IMU_calibration",responseCalibrateIMU); - - - // New messages are sent from the sensor with sample_rate - // As loop_rate determines how often we check for new data - // on the serial buffer, theoretically loop_rate = sample_rate - // should be okey, but to be sure we double it - ros::Rate loop_rate(sample_rate * 2); - - try { - SerialUnix serial_driver(device_name, stim_const::BaudRate::BAUD_921600); - DriverStim300 driver_stim300(serial_driver); - - ROS_INFO("STIM300 IMU driver initialized successfully"); - - int difference_in_dataGram{0}; - int count_messages{0}; - int number_of_samples{0}; - double inclination_x{0}; - double inclination_y{0}; - double inclination_z{0}; - - double average_calibration_roll{0}; - double average_calibration_pitch{0}; - double inclination_x_calibration_sum{0}; - double inclination_y_calibration_sum{0}; - double inclination_z_calibration_sum{0}; - double inclination_x_average{0}; - double inclination_y_average{0}; - double inclination_z_average{0}; - - // Acc wild point filter - std::vector acceleration_buffer_x{}; - std::vector acceleration_buffer_y{}; - std::vector acceleration_buffer_z{}; - double dropped_acceleration_x_msg{0.0}; - double dropped_acceleration_y_msg{0.0}; - double dropped_acceleration_z_msg{0.0}; - - // Gyro wild point filter - std::vector gyro_buffer_x{}; - std::vector gyro_buffer_y{}; - std::vector gyro_buffer_z{}; - double dropped_gyro_x_msg{0.0}; - double dropped_gyro_y_msg{0.0}; - double dropped_gyro_z_msg{0.0}; - - - - - while (ros::ok()) - { - switch (driver_stim300.update()) - { - case Stim300Status::NORMAL: - break; - case Stim300Status::OUTSIDE_OPERATING_CONDITIONS: - ROS_DEBUG("Stim 300 outside operating conditions"); - break; - case Stim300Status::NEW_MEASURMENT: - inclination_x = driver_stim300.getIncX(); - inclination_y = driver_stim300.getIncY(); - inclination_z = driver_stim300.getIncZ(); - Quaternion q; - EulerAngles RPY; - if (calibration_mode == true) - { - //std::cout<<"in calibration_mode"< MAX_DROPPED_ACC_X_MSG ) - { - stim300msg.linear_acceleration.x = acceleration_buffer_x.back(); - dropped_acceleration_x_msg = 0; - } - else - { - ROS_DEBUG("ACC_X_MSG wild point rejected"); - dropped_acceleration_x_msg +=1; - } - - if(std::abs(acceleration_buffer_y.back() - acceleration_buffer_y.front()) < ACC_TOLERANCE || dropped_acceleration_y_msg > MAX_DROPPED_ACC_Y_MSG ) - { - stim300msg.linear_acceleration.y = acceleration_buffer_y.back(); - dropped_acceleration_y_msg = 0; - } - else - { - ROS_DEBUG("ACC_Y_MSG wild point rejected"); - dropped_acceleration_y_msg +=1; - } - - if(std::abs(acceleration_buffer_z.back() - acceleration_buffer_z.front()) < ACC_TOLERANCE || dropped_acceleration_z_msg > MAX_DROPPED_ACC_Z_MSG ) - { - stim300msg.linear_acceleration.z = acceleration_buffer_z.back(); - dropped_acceleration_z_msg = 0; - } - else - { - ROS_DEBUG("ACC_Z_MSG wild point rejected"); - dropped_acceleration_z_msg +=1; - } - // Empty acceleration buffers - acceleration_buffer_x = std::vector{acceleration_buffer_x.back()}; - acceleration_buffer_y = std::vector{acceleration_buffer_y.back()}; - acceleration_buffer_z = std::vector{acceleration_buffer_z.back()}; - } - else - { - stim300msg.linear_acceleration.x = driver_stim300.getAccX() * gravity; - stim300msg.linear_acceleration.y = driver_stim300.getAccY() * gravity; - stim300msg.linear_acceleration.z = driver_stim300.getAccZ() * gravity; - } - - // Gyro wild point filter - gyro_buffer_x.push_back(driver_stim300.getGyroX()); - gyro_buffer_y.push_back(driver_stim300.getGyroY()); - gyro_buffer_z.push_back(driver_stim300.getGyroZ()); - - if(gyro_buffer_x.size() == 2 && gyro_buffer_y.size() == 2 && gyro_buffer_z.size() == 2) - { - - if(std::abs(gyro_buffer_x.back() - gyro_buffer_x.front()) < std::max(2*std::abs(gyro_buffer_x.front()),GYRO_X_PEAK_TO_PEAK_NOISE) || dropped_gyro_x_msg > MAX_DROPPED_GYRO_X_MSG) - { - stim300msg.angular_velocity.x = gyro_buffer_x.back(); - dropped_gyro_x_msg = 0; - } - else - { - ROS_DEBUG("GYRO_X_MSG wild point rejected"); - dropped_gyro_x_msg += 1; - } - - if(std::abs(gyro_buffer_y.back() - gyro_buffer_y.front()) < std::max(2*std::abs(gyro_buffer_y.front()),GYRO_Y_PEAK_TO_PEAK_NOISE) || dropped_gyro_y_msg > MAX_DROPPED_GYRO_Y_MSG) - { - stim300msg.angular_velocity.x = gyro_buffer_y.back(); - dropped_gyro_y_msg = 0; - } - else - { - ROS_DEBUG("GYRO_Y_MSG wild point rejected"); - dropped_gyro_y_msg += 1; - } - - if(std::abs(gyro_buffer_z.back() - gyro_buffer_z.front()) < std::max(2*std::abs(gyro_buffer_z.front()),GYRO_Z_PEAK_TO_PEAK_NOISE) || dropped_gyro_z_msg > MAX_DROPPED_GYRO_Z_MSG) - { - stim300msg.angular_velocity.z = gyro_buffer_z.back(); - dropped_gyro_z_msg = 0; - } - else - { - ROS_DEBUG("GYRO_Z_MSG wild point rejected"); - dropped_gyro_z_msg += 1; - } - - // Empty buffers - gyro_buffer_x = std::vector{gyro_buffer_x.back()}; - gyro_buffer_y = std::vector{gyro_buffer_y.back()}; - gyro_buffer_z = std::vector{gyro_buffer_z.back()}; - } - else - { - stim300msg.angular_velocity.x = driver_stim300.getGyroX(); - stim300msg.angular_velocity.y = driver_stim300.getGyroY(); - stim300msg.angular_velocity.z = driver_stim300.getGyroZ(); - } - stim300msg.orientation.w = q.w; - stim300msg.orientation.x = q.x; - stim300msg.orientation.y = q.y; - stim300msg.orientation.z = q.z; - imuSensorPublisher.publish(stim300msg); - break; - } - case Stim300Status::CONFIG_CHANGED: - ROS_INFO("Updated Stim 300 imu config: "); - ROS_INFO("%s", driver_stim300.printSensorConfig().c_str()); - loop_rate = driver_stim300.getSampleRate()*2; - break; - case Stim300Status::STARTING_SENSOR: - ROS_INFO("Stim 300 IMU is warming up."); - break; - case Stim300Status::SYSTEM_INTEGRITY_ERROR: - ROS_WARN("Stim 300 IMU system integrity error."); - break; - case Stim300Status::OVERLOAD: - ROS_WARN("Stim 300 IMU overload."); - break; - case Stim300Status::ERROR_IN_MEASUREMENT_CHANNEL: - ROS_WARN("Stim 300 IMU error in measurement channel."); - break; - case Stim300Status::ERROR: - ROS_WARN("Stim 300 IMU: internal error."); - - } - - loop_rate.sleep(); - ros::spinOnce(); - } - return 0; - } catch (std::runtime_error &error) { - // TODO: Reset IMU - ROS_ERROR("%s\n", error.what()); - return 0; - } -} \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..9d9f2a1 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,38 @@ +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() + +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) + +ament_add_gtest(check_datasheet_constanst +${CMAKE_CURRENT_SOURCE_DIR}/check_stim300_constants.cpp +) +target_include_directories(check_datasheet_constanst + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../include +) +target_link_libraries(check_datasheet_constanst + stim300_driver_lib +) +ament_target_dependencies(check_datasheet_constanst + rclcpp + sensor_msgs + std_srvs +) + +ament_add_gmock(check_driver_stim300 + ${CMAKE_CURRENT_SOURCE_DIR}/check_driver_stim300.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/mock_serial_driver.h +) +target_include_directories(check_driver_stim300 + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../include +) +target_link_libraries(check_driver_stim300 + stim300_driver_lib +) +ament_target_dependencies(check_driver_stim300 + rclcpp + sensor_msgs + std_srvs +) \ No newline at end of file diff --git a/test/check_driver_stim300.cpp b/test/check_driver_stim300.cpp index 6f121f1..bf7c6b8 100644 --- a/test/check_driver_stim300.cpp +++ b/test/check_driver_stim300.cpp @@ -1,5 +1,5 @@ -#include "../include/driver_stim300.h" +#include "stim300_driver/driver_stim300.hpp" #include "mock_serial_driver.h" #include "gmock/gmock.h" diff --git a/test/check_stim300_constants.cpp b/test/check_stim300_constants.cpp index 01d9737..47a27d6 100644 --- a/test/check_stim300_constants.cpp +++ b/test/check_stim300_constants.cpp @@ -1,4 +1,4 @@ -#include "../src/stim300_constants.h" +#include "stim300_driver/stim300_constants.hpp" #include #include diff --git a/test/mock_serial_driver.h b/test/mock_serial_driver.h index 320eca3..e9fab20 100644 --- a/test/mock_serial_driver.h +++ b/test/mock_serial_driver.h @@ -2,8 +2,8 @@ #ifndef DRIVER_STIM300_MOCK_SERIAL_DRIVER_H #define DRIVER_STIM300_MOCK_SERIAL_DRIVER_H -#include "../src/serial_unix.h" -#include "../src/stim300_constants.h" +#include "stim300_driver/serial_unix.hpp" +#include "stim300_driver/stim300_constants.hpp" #include "gmock/gmock.h" #include