diff --git a/.clang-format b/.clang-format index 33bf2a3..271a184 100644 --- a/.clang-format +++ b/.clang-format @@ -132,6 +132,4 @@ StatementMacros: - QT_REQUIRE_VERSION TabWidth: 8 UseCRLF: false -UseTab: Never -... - +UseTab: Never \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index ab1d583..0ab5eae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,8 @@ install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY launch +install( + DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) diff --git a/include/ros_stim300_driver/ros_stim300_driver.hpp b/include/ros_stim300_driver/ros_stim300_driver.hpp index 07330f4..82a4d9a 100644 --- a/include/ros_stim300_driver/ros_stim300_driver.hpp +++ b/include/ros_stim300_driver/ros_stim300_driver.hpp @@ -1,13 +1,13 @@ #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 "iostream" #include "math.h" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_srvs/srv/trigger.hpp" -#include "iostream" +#include "stim300_driver/driver_stim300.hpp" +#include "stim300_driver/serial_unix.hpp" #include constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; @@ -22,45 +22,44 @@ 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 Quaternion { + double w, x, y, z; }; struct EulerAngles { - double roll, pitch, yaw; + double roll, pitch, yaw; }; -Quaternion FromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X) - +Quaternion +FromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X) -class Stim300DriverNode : public rclcpp::Node -{ +class Stim300DriverNode : public rclcpp::Node { public: - Stim300DriverNode(); + Stim300DriverNode(); private: - void timerCallback(); + void timerCallback(); - bool responseCalibrateIMU(const std::shared_ptr request, - std::shared_ptr response); + 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_; + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Service::SharedPtr calibration_service_; + rclcpp::TimerBase::SharedPtr timer_; - bool calibration_mode_ = false; + bool calibration_mode_ = false; - std::string device_name_; - double variance_gyro_; - double variance_acc_; - double gravity_; - double sample_rate_; + std::string device_name_; + double variance_gyro_; + double variance_acc_; + double gravity_; + double sample_rate_; - sensor_msgs::msg::Imu stim300msg_; + sensor_msgs::msg::Imu stim300msg_; - std::unique_ptr serial_driver_; - std::unique_ptr driver_stim300_; + std::unique_ptr serial_driver_; + std::unique_ptr driver_stim300_; }; -#endif // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP \ No newline at end of file +#endif // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP \ No newline at end of file diff --git a/launch/stim300_driver.launch.py b/launch/stim300_driver.launch.py index 3cf3a66..912b94e 100644 --- a/launch/stim300_driver.launch.py +++ b/launch/stim300_driver.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): ), Node( package='driver_stim300', - executable='stim300_driver_node', + executable='ros_stim300_driver_node', name='stim300driver', parameters=[{ 'device_name': LaunchConfiguration('device_name'), diff --git a/src/ros_stim300_driver/ros_stim300_driver.cpp b/src/ros_stim300_driver/ros_stim300_driver.cpp index 85a4f67..4a1955a 100644 --- a/src/ros_stim300_driver/ros_stim300_driver.cpp +++ b/src/ros_stim300_driver/ros_stim300_driver.cpp @@ -1,292 +1,296 @@ #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 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; - 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; + 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); +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_); + 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)); + 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"; + 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"); + 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()); - } + 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}; +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}; + 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}; + // 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}; + // 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; + 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); + 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(); + // 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 (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_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_; - } + 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()); + // 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 (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_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; - } + 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(); - } + // 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."); + 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; +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 index b161088..6140bef 100644 --- a/src/ros_stim300_driver/ros_stim300_driver_node.cpp +++ b/src/ros_stim300_driver/ros_stim300_driver_node.cpp @@ -1,10 +1,9 @@ #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; +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