98 lines
2.4 KiB
C++
98 lines
2.4 KiB
C++
#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
|