4444#include " sensor_msgs/msg/magnetic_field.hpp"
4545#include " std_msgs/msg/float64.hpp"
4646
47- class RtUsb9axisimuRosDriver : public rt_usb_9axisimu ::SerialPort
47+ class RtUsb9axisimuRosDriver
4848{
49- private:
50- // ros::NodeHandle nh_;
49+ public:
50+ explicit RtUsb9axisimuRosDriver (std::string serialport);
51+ RtUsb9axisimuRosDriver (std::unique_ptr<rt_usb_9axisimu::SerialPort> serial_port);
52+ ~RtUsb9axisimuRosDriver ();
5153
52- // ros::Publisher imu_data_raw_pub_;
53- // ros::Publisher imu_mag_pub_;
54- // ros::Publisher imu_temperature_pub_;
54+ enum ReadStatus
55+ {
56+ SUCCESS = 0 ,
57+ NEED_TO_CONTINUE,
58+ FAILURE
59+ };
60+
61+ void setImuFrameIdName (std::string frame_id);
62+ void setImuPortName (std::string port);
63+ void setImuStdDev (double linear_acceleration, double angular_velocity, double magnetic_field);
64+
65+ bool startCommunication ();
66+ void stopCommunication (void );
67+ void checkDataFormat (const double timeout = 5.0 );
68+ bool hasAsciiDataFormat (void );
69+ bool hasBinaryDataFormat (void );
70+ bool hasRefreshedImuData (void );
71+
72+ std::unique_ptr<sensor_msgs::msg::Imu> getImuRawDataUniquePtr (const rclcpp::Time timestamp);
73+ std::unique_ptr<sensor_msgs::msg::MagneticField> getImuMagUniquePtr (const rclcpp::Time timestamp);
74+ std::unique_ptr<std_msgs::msg::Float64> getImuTemperatureUniquePtr (void );
75+ ReadStatus readSensorData ();
76+
77+ private:
78+ std::unique_ptr<rt_usb_9axisimu::SerialPort> serial_port_;
5579
5680 rt_usb_9axisimu::SensorData sensor_data_;
5781
@@ -61,6 +85,11 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
6185 double magnetic_field_stddev_;
6286 rt_usb_9axisimu::Consts consts;
6387
88+ unsigned char bin_read_buffer_[rt_usb_9axisimu::Consts::READ_BUFFER_SIZE];
89+ unsigned char ascii_read_buffer_[rt_usb_9axisimu::Consts::READ_BUFFER_SIZE];
90+ unsigned int bin_read_buffer_idx_ = 0 ;
91+ unsigned int ascii_read_buffer_idx_ = 0 ;
92+
6493 enum DataFormat
6594 {
6695 NONE = 0 ,
@@ -70,39 +99,18 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
7099 ASCII,
71100 INCORRECT
72101 };
73- bool has_completed_format_check_;
74102 DataFormat data_format_;
75103 bool has_refreshed_imu_data_;
76104
77105 // Method to combine two separate one-byte data into one two-byte data
78106 int16_t combineByteData (unsigned char data_h, unsigned char data_l);
79107 // Method to extract binary sensor data from communication buffer
80108 rt_usb_9axisimu::ImuData<int16_t > extractBinarySensorData (unsigned char * imu_data_buf);
81- bool isBinarySensorData (unsigned char * imu_data_buf);
82- bool readBinaryData (void );
109+ bool isBinarySensorData (unsigned char * imu_data_buf, unsigned int data_size);
110+ ReadStatus readBinaryData (void );
111+ bool isAsciiSensorData (unsigned char * imu_data_buf, unsigned int data_size);
83112 bool isValidAsciiSensorData (std::vector<std::string> imu_data_vector_buf);
84- bool readAsciiData (void );
85-
86- public:
87- explicit RtUsb9axisimuRosDriver (std::string serialport);
88- ~RtUsb9axisimuRosDriver ();
89-
90- void setImuFrameIdName (std::string frame_id);
91- void setImuPortName (std::string port);
92- void setImuStdDev (double linear_acceleration, double angular_velocity, double magnetic_field);
93-
94- bool startCommunication ();
95- void stopCommunication (void );
96- void checkDataFormat (void );
97- bool hasCompletedFormatCheck (void );
98- bool hasAsciiDataFormat (void );
99- bool hasBinaryDataFormat (void );
100- bool hasRefreshedImuData (void );
101-
102- std::unique_ptr<sensor_msgs::msg::Imu> getImuRawDataUniquePtr (const rclcpp::Time timestamp);
103- std::unique_ptr<sensor_msgs::msg::MagneticField> getImuMagUniquePtr (const rclcpp::Time timestamp);
104- std::unique_ptr<std_msgs::msg::Float64> getImuTemperatureUniquePtr (void );
105- bool readSensorData ();
113+ ReadStatus readAsciiData (void );
106114};
107115
108116#endif // RT_USB_9AXISIMU_DRIVER__RT_USB_9AXISIMU_DRIVER_HPP_
0 commit comments