Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,97 @@
+#ifndef BASE_DRIVER_H_
+#define BASE_DRIVER_H_
+
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <iostream>
+#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
+
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+
+#include <math.h>
+#include <fstream>
+#include <fdilink_data_struct.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <geometry_msgs/Pose2D.h>
+#include <boost/thread.hpp>
+#include <string>
+#include <ros/package.h>
+#include <crc_table.h>
+
+using namespace std;
+namespace FDILink
+{
+#define FRAME_HEAD 0xfc
+#define FRAME_END 0xfd
+#define TYPE_IMU 0x40
+#define TYPE_AHRS 0x41
+#define TYPE_INSGPS 0x42
+#define TYPE_GEODETIC_POS 0x5c
+#define TYPE_GROUND 0xf0
+
+#define IMU_LEN  0x38   //56
+#define AHRS_LEN 0x30   //48
+#define INSGPS_LEN 0x48 //80
+#define GEODETIC_POS_LEN 0x20 //32
+#define PI 3.141592653589793
+#define DEG_TO_RAD 0.017453292519943295
+
+
+class ahrsBringup
+{
+public:
+  ahrsBringup();
+  ~ahrsBringup();
+  void processLoop();
+  bool checkCS8(int len);
+  bool checkCS16(int len);
+  void checkSN(int type);
+  void magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz);
+  ros::NodeHandle nh_;
+
+private:
+  bool if_debug_;
+  //sum info
+  int sn_lost_ = 0;
+  int crc_error_ = 0;
+  uint8_t read_sn_ = 0;
+  bool frist_sn_;
+  int device_type_ = 1;
+
+  //serial
+  serial::Serial serial_; //声明串口对象
+  std::string serial_port_;
+  int serial_baud_;
+  int serial_timeout_;
+  //data
+  FDILink::imu_frame_read  imu_frame_;
+  FDILink::ahrs_frame_read ahrs_frame_;
+  FDILink::insgps_frame_read insgps_frame_;
+  //FDILink::lanlon_frame_read latlon_frame_;
+  FDILink::Geodetic_Position_frame_read Geodetic_Position_frame_;
+  //frame name
+  string imu_frame_id_;
+  string insgps_frame_id_;
+  string latlon_frame_id_;
+  //topic
+  string imu_topic_, mag_pose_2d_topic_;
+  string latlon_topic_;
+  string Euler_angles_topic_,Magnetic_topic_;
+  string gps_topic_,twist_topic_,NED_odom_topic_;
+
+  //Publisher
+  ros::Publisher imu_pub_;
+  ros::Publisher gps_pub_;
+  ros::Publisher mag_pose_pub_;
+  ros::Publisher Euler_angles_pub_;
+  ros::Publisher Magnetic_pub_;
+  ros::Publisher twist_pub_;
+  ros::Publisher NED_odom_pub_;
+
+
+}; //ahrsBringup
+} // namespace FDILink
+
+#endif