inertial_sense.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <stdio.h>
4 #include <iostream>
5 #include <algorithm>
6 #include <string>
7 #include <cstdlib>
8 
9 #include "InertialSense.h"
10 
11 #include "ros/ros.h"
12 #include "ros/timer.h"
13 #include "sensor_msgs/Imu.h"
14 #include "sensor_msgs/MagneticField.h"
15 #include "sensor_msgs/FluidPressure.h"
16 #include "sensor_msgs/JointState.h"
17 #include "inertial_sense/GPS.h"
18 #include "inertial_sense/GPSInfo.h"
19 #include "inertial_sense/PreIntIMU.h"
20 #include "inertial_sense/FirmwareUpdate.h"
21 #include "inertial_sense/refLLAUpdate.h"
22 #include "inertial_sense/RTKRel.h"
23 #include "inertial_sense/RTKInfo.h"
24 #include "inertial_sense/GNSSEphemeris.h"
25 #include "inertial_sense/GlonassEphemeris.h"
26 #include "inertial_sense/GNSSObservation.h"
27 #include "inertial_sense/GNSSObsVec.h"
28 #include "inertial_sense/INL2States.h"
29 #include "nav_msgs/Odometry.h"
30 #include "std_srvs/Trigger.h"
31 #include "std_msgs/Header.h"
32 #include "geometry_msgs/Vector3Stamped.h"
33 #include "geometry_msgs/PoseWithCovarianceStamped.h"
34 #include "diagnostic_msgs/DiagnosticArray.h"
36 //#include "geometry/xform.h"
37 
38 # define GPS_UNIX_OFFSET 315964800 // GPS time started on 6/1/1980 while UNIX time started 1/1/1970 this is the difference between those in seconds
39 # define LEAP_SECONDS 18 // GPS time does not have leap seconds, UNIX does (as of 1/1/2017 - next one is probably in 2020 sometime unless there is some crazy earthquake or nuclear blast)
40 # define UNIX_TO_GPS_OFFSET (GPS_UNIX_OFFSET - LEAP_SECONDS)
41 
42 #define SET_CALLBACK(DID, __type, __cb_fun, __periodmultiple) \
43  IS_.BroadcastBinaryData(DID, __periodmultiple, \
44  [this](InertialSense*i, p_data_t* data, int pHandle)\
45  { \
46  /* ROS_INFO("Got message %d", DID);*/\
47  this->__cb_fun(reinterpret_cast<__type*>(data->buf));\
48  })
49 
50 
51 class InertialSenseROS //: SerialListener
52 {
53 public:
54  typedef enum
55  {
56  NMEA_GPGGA = 0x01,
57  NMEA_GPGLL = 0x02,
58  NMEA_GPGSA = 0x04,
59  NMEA_GPRMC = 0x08,
60  NMEA_SER0 = 0x01,
61  NMEA_SER1 = 0x02
63 
65  void callback(p_data_t* data);
66  void update();
67 
68  void connect();
69  void set_navigation_dt_ms();
70  void configure_parameters();
71  void configure_rtk();
74  void start_log();
75 
76  template<typename T> void set_vector_flash_config(std::string param_name, uint32_t size, uint32_t offset);
77  template<typename T> void set_flash_config(std::string param_name, uint32_t offset, T def) __attribute__ ((optimize(0)));
78  void get_flash_config();
79  void reset_device();
80  void flash_config_callback(const nvm_flash_cfg_t* const msg);
81  // Serial Port Configuration
82  std::string port_;
83  int baudrate_;
86 
87  std::string frame_id_;
88 
89  // ROS Stream handling
90  typedef struct
91  {
92  bool enabled;
95  } ros_stream_t;
96 
98  void INS1_callback(const ins_1_t* const msg);
99  void INS2_callback(const ins_2_t* const msg);
100 // void INS_variance_callback(const inl2_variance_t* const msg);
101 
103  void INL2_states_callback(const inl2_states_t* const msg);
105  bool publishTf;
107  int LTCF;
108  enum
109  {
112  }ltcf;
113 
115  void IMU_callback(const dual_imu_t* const msg);
116 
120  void GPS_pos_callback(const gps_pos_t* const msg);
121  void GPS_vel_callback(const gps_vel_t* const msg);
122  void GPS_raw_callback(const gps_raw_t* const msg);
123  void GPS_obs_callback(const obsd_t * const msg, int nObs);
124  void GPS_eph_callback(const eph_t* const msg);
125  void GPS_geph_callback(const geph_t* const msg);
127  inertial_sense::GNSSObsVec obs_Vec_;
130 
132  void GPS_info_callback(const gps_sat_t* const msg);
133 
135  void mag_callback(const magnetometer_t* const msg);
136 
138  void baro_callback(const barometer_t* const msg);
139 
141  void preint_IMU_callback(const preintegrated_imu_t * const msg);
142 
144  void strobe_in_time_callback(const strobe_in_time_t * const msg);
145 
147  void diagnostics_callback(const ros::TimerEvent& event);
150 
156  bool set_current_position_as_refLLA(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response & res);
157  bool set_refLLA_to_value(inertial_sense::refLLAUpdate::Request &req, inertial_sense::refLLAUpdate::Response &res);
158  bool perform_mag_cal_srv_callback(std_srvs::Trigger::Request & req, std_srvs::Trigger::Response & res);
159  bool perform_multi_mag_cal_srv_callback(std_srvs::Trigger::Request & req, std_srvs::Trigger::Response & res);
160  bool update_firmware_srv_callback(inertial_sense::FirmwareUpdate::Request & req, inertial_sense::FirmwareUpdate::Response & res);
161 
162  void publishGPS();
163 
164  typedef enum
165  {
170  } rtk_state_t;
171  rtk_state_t RTK_state_ = RTK_NONE;
173  void RTK_Misc_callback(const gps_rtk_misc_t* const msg);
174  void RTK_Rel_callback(const gps_rtk_rel_t* const msg);
175 
176 
184  ros::Time ros_time_from_week_and_tow(const uint32_t week, const double timeOfWeek);
185 
191  ros::Time ros_time_from_start_time(const double time);
192 
199  ros::Time ros_time_from_tow(const double tow);
200 
201  double tow_from_ros_time(const ros::Time& rt);
202  ros::Time ros_time_from_gtime(const uint64_t sec, double subsec);
203 
204  double GPS_towOffset_ = 0; // The offset between GPS time-of-week and local time on the uINS
205  // If this number is 0, then we have not yet got a fix
206  uint64_t GPS_week_ = 0; // Week number to start of GPS_towOffset_ in GPS time
207  // Time sync variables
208  double INS_local_offset_ = 0.0; // Current estimate of the uINS start time in ROS time seconds
209  bool got_first_message_ = false; // Flag to capture first uINS start time guess
210 
211  // Data to hold on to in between callbacks
212  double lla_[3];
213  double ecef_[3];
214  sensor_msgs::Imu imu1_msg, imu2_msg;
215  nav_msgs::Odometry odom_msg;
216  inertial_sense::GPS gps_msg;
217  geometry_msgs::Vector3Stamped gps_velEcef;
218  inertial_sense::GPSInfo gps_info_msg;
219  inertial_sense::INL2States inl2_states_msg;
220 
223 
224  // Connection to the uINS
226 };
tf::TransformBroadcaster br
ros_stream_t GPS_eph_
bool update_firmware_srv_callback(inertial_sense::FirmwareUpdate::Request &req, inertial_sense::FirmwareUpdate::Response &res)
ros::Publisher strobe_pub_
ros::NodeHandle nh_private_
ros::Time ros_time_from_gtime(const uint64_t sec, double subsec)
enum InertialSenseROS::@0 ltcf
ros_stream_t GPS_info_
std::string frame_id_
inertial_sense::INL2States inl2_states_msg
void RTK_Rel_callback(const gps_rtk_rel_t *const msg)
void mag_callback(const magnetometer_t *const msg)
void GPS_raw_callback(const gps_raw_t *const msg)
void preint_IMU_callback(const preintegrated_imu_t *const msg)
geometry_msgs::Vector3Stamped gps_velEcef
void GPS_info_callback(const gps_sat_t *const msg)
float diagnostic_heading_base_to_rover_
float diagnostic_differential_age_
void GPS_pos_callback(const gps_pos_t *const msg)
void INS1_callback(const ins_1_t *const msg)
void RTK_Misc_callback(const gps_rtk_misc_t *const msg)
ros::Timer obs_bundle_timer_
inertial_sense::GPS gps_msg
ros::Time last_obs_time_
void get_flash_config()
void diagnostics_callback(const ros::TimerEvent &event)
ros_stream_t IMU_
nav_msgs::Odometry odom_msg
ros::ServiceServer firmware_update_srv_
double tow_from_ros_time(const ros::Time &rt)
void GPS_vel_callback(const gps_vel_t *const msg)
void strobe_in_time_callback(const strobe_in_time_t *const msg)
void INS2_callback(const ins_2_t *const msg)
ros::ServiceServer mag_cal_srv_
sensor_msgs::Imu imu2_msg
ros::Timer diagnostics_timer_
ros::Time ros_time_from_start_time(const double time)
ros_time_from_start_time
void set_flash_config(std::string param_name, uint32_t offset, T def) __attribute__((optimize(0)))
void set_vector_flash_config(std::string param_name, uint32_t size, uint32_t offset)
ros_stream_t GPS_
typedef __attribute__
USB Device LPM Descriptor structure.
Definition: d_usartDMA.c:1064
void GPS_obs_callback(const obsd_t *const msg, int nObs)
ros::ServiceServer multi_mag_cal_srv_
USBInterfaceDescriptor data
ros::Time ros_time_from_tow(const double tow)
ros_time_from_tow Get equivalent ros time from tow and internal week counter
void GPS_geph_callback(const geph_t *const msg)
ros_stream_t INL2_states_
ros_stream_t RTK_
inertial_sense::GPSInfo gps_info_msg
rtk_state_t RTK_state_
ros_stream_t mag_
ros::ServiceServer refLLA_set_current_srv_
bool perform_mag_cal_srv_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
sensor_msgs::Imu imu1_msg
void GPS_eph_callback(const eph_t *const msg)
void configure_data_streams()
ros::Time ros_time_from_week_and_tow(const uint32_t week, const double timeOfWeek)
ros_time_from_week_and_tow Get current ROS time from week and tow
void GPS_obs_bundle_timer_callback(const ros::TimerEvent &e)
bool set_current_position_as_refLLA(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
ros_stream_t INS_
ros::ServiceServer refLLA_set_value_srv_
ros_stream_t GPS_obs_
std::string port_
ros_stream_t baro_
ros_stream_t dt_vel_
tf::Transform transform
void IMU_callback(const dual_imu_t *const msg)
void baro_callback(const barometer_t *const msg)
void callback(p_data_t *data)
bool set_refLLA_to_value(inertial_sense::refLLAUpdate::Request &req, inertial_sense::refLLAUpdate::Response &res)
ros::NodeHandle nh_
inertial_sense::GNSSObsVec obs_Vec_
InertialSense IS_
bool perform_multi_mag_cal_srv_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
void flash_config_callback(const nvm_flash_cfg_t *const msg)
ros_stream_t diagnostics_
void INL2_states_callback(const inl2_states_t *const msg)


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04