Skip to content

Commit

Permalink
Formatting
Browse files Browse the repository at this point in the history
Signed-off-by: Christopher Strøm <[email protected]>
  • Loading branch information
chrstrom committed Jun 22, 2024
1 parent e859ecd commit c6cfb0b
Show file tree
Hide file tree
Showing 6 changed files with 300 additions and 299 deletions.
4 changes: 1 addition & 3 deletions .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,4 @@ StatementMacros:
- QT_REQUIRE_VERSION
TabWidth: 8
UseCRLF: false
UseTab: Never
...

UseTab: Never
3 changes: 2 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(DIRECTORY launch
install(
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)

Expand Down
55 changes: 27 additions & 28 deletions include/ros_stim300_driver/ros_stim300_driver.hpp
Original file line number Diff line number Diff line change
@@ -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 <vector>

constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100};
Expand All @@ -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<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response);
bool responseCalibrateIMU(
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response);

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr calibration_service_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::Service<std_srvs::srv::Trigger>::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<SerialUnix> serial_driver_;
std::unique_ptr<DriverStim300> driver_stim300_;
std::unique_ptr<SerialUnix> serial_driver_;
std::unique_ptr<DriverStim300> driver_stim300_;
};

#endif // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP
#endif // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP
2 changes: 1 addition & 1 deletion launch/stim300_driver.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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'),
Expand Down
Loading

0 comments on commit c6cfb0b

Please sign in to comment.