22 MessageWrapper::MessageWrapper(
void):
23 m_first_valid_utc_(false)
39 angle_rad = fmodf(angle_rad,
SBG_PI_F * 2.0f);
44 angle_rad =
SBG_PI_F * 2.0f + angle_rad;
52 if ( (angle_deg < -360.0
f) || (angle_deg > 360.0
f) )
54 angle_deg = fmodf(angle_deg, 360.0
f);
59 angle_deg = 360.0f + angle_deg;
67 return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0;
95 uint32_t device_timestamp_diff;
101 nanoseconds = utc_to_epoch.
toNSec() +
static_cast<uint64_t
>(device_timestamp_diff) * 1000;
110 sbg_driver::SbgEkfStatus ekf_status_message;
133 return ekf_status_message;
138 sbg_driver::SbgGpsPosStatus gps_pos_status_message;
150 return gps_pos_status_message;
155 sbg_driver::SbgGpsVelStatus gps_vel_status_message;
160 return gps_vel_status_message;
165 sbg_driver::SbgImuStatus imu_status_message;
180 return imu_status_message;
185 sbg_driver::SbgMagStatus mag_status_message;
199 return mag_status_message;
204 sbg_driver::SbgShipMotionStatus ship_motion_status_message;
211 return ship_motion_status_message;
216 sbg_driver::SbgStatusAiding status_aiding_message;
227 return status_aiding_message;
232 sbg_driver::SbgStatusCom status_com_message;
255 return status_com_message;
260 sbg_driver::SbgStatusGeneral status_general_message;
268 return status_general_message;
273 sbg_driver::SbgUtcTimeStatus utc_status_message;
281 return utc_status_message;
298 if ((month_index == 4) || (month_index == 6) || (month_index == 9) || (month_index == 11))
302 else if ((month_index == 2))
321 return ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0);
328 uint64_t nanoseconds;
336 for (uint16_t yearIndex = 1970; yearIndex < ref_sbg_utc_msg.year; yearIndex++)
341 for (uint8_t monthIndex = 1; monthIndex < ref_sbg_utc_msg.month; monthIndex++)
346 days += ref_sbg_utc_msg.day - 1;
348 nanoseconds = days * 24;
349 nanoseconds = (nanoseconds + ref_sbg_utc_msg.hour) * 60;
350 nanoseconds = (nanoseconds + ref_sbg_utc_msg.min) * 60;
351 nanoseconds = nanoseconds + ref_sbg_utc_msg.sec;
352 nanoseconds = nanoseconds * 1000000000 + ref_sbg_utc_msg.nanosec;
361 sbg_driver::SbgAirDataStatus air_data_status_message;
370 return air_data_status_message;
382 char LetterDesignator;
384 if ((84 >= Lat) && (Lat >= 72)) LetterDesignator =
'X';
385 else if ((72 > Lat) && (Lat >= 64)) LetterDesignator =
'W';
386 else if ((64 > Lat) && (Lat >= 56)) LetterDesignator =
'V';
387 else if ((56 > Lat) && (Lat >= 48)) LetterDesignator =
'U';
388 else if ((48 > Lat) && (Lat >= 40)) LetterDesignator =
'T';
389 else if ((40 > Lat) && (Lat >= 32)) LetterDesignator =
'S';
390 else if ((32 > Lat) && (Lat >= 24)) LetterDesignator =
'R';
391 else if ((24 > Lat) && (Lat >= 16)) LetterDesignator =
'Q';
392 else if ((16 > Lat) && (Lat >= 8)) LetterDesignator =
'P';
393 else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator =
'N';
394 else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator =
'M';
395 else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator =
'L';
396 else if((-16 > Lat) && (Lat >= -24)) LetterDesignator =
'K';
397 else if((-24 > Lat) && (Lat >= -32)) LetterDesignator =
'J';
398 else if((-32 > Lat) && (Lat >= -40)) LetterDesignator =
'H';
399 else if((-40 > Lat) && (Lat >= -48)) LetterDesignator =
'G';
400 else if((-48 > Lat) && (Lat >= -56)) LetterDesignator =
'F';
401 else if((-56 > Lat) && (Lat >= -64)) LetterDesignator =
'E';
402 else if((-64 > Lat) && (Lat >= -72)) LetterDesignator =
'D';
403 else if((-72 > Lat) && (Lat >= -80)) LetterDesignator =
'C';
405 else LetterDesignator =
'Z';
406 return LetterDesignator;
414 double LongTemp = (Long+180)-
int((Long+180)/360)*360-180;
416 zoneNumber = int((LongTemp + 180)/6) + 1;
418 if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
424 if( Lat >= 72.0 && Lat < 84.0 )
426 if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31;
427 else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33;
428 else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35;
429 else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37;
436 ROS_INFO(
"initialized from lat:%f long:%f UTM zone %d%c: easting:%fm (%dkm) northing:%fm (%dkm)" 456 const double RADIANS_PER_DEGREE = M_PI/180.0;
459 const double WGS84_A = 6378137.0;
460 const double WGS84_E = 0.0818191908;
463 const double UTM_K0 = 0.9996;
464 const double UTM_E2 = (WGS84_E*WGS84_E);
467 double eccSquared = UTM_E2;
471 double eccPrimeSquared;
472 double N, T, C, A, M;
475 double LongTemp = (Long+180)-
int((Long+180)/360)*360-180;
477 double LatRad = Lat*RADIANS_PER_DEGREE;
478 double LongRad = LongTemp*RADIANS_PER_DEGREE;
479 double LongOriginRad;
482 LongOrigin = (zoneNumber - 1)*6 - 180 + 3;
483 LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
485 eccPrimeSquared = (eccSquared)/(1-eccSquared);
487 N = a/
sqrt(1-eccSquared*
sin(LatRad)*
sin(LatRad));
488 T =
tan(LatRad)*
tan(LatRad);
489 C = eccPrimeSquared*
cos(LatRad)*
cos(LatRad);
490 A = cos(LatRad)*(LongRad-LongOriginRad);
492 M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad
493 - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*
sin(2*LatRad)
494 + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*
sin(4*LatRad)
495 - (35*eccSquared*eccSquared*eccSquared/3072)*
sin(6*LatRad));
497 UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
498 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
501 UTMNorthing = (double)(k0*(M+N*
tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
502 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
506 UTMNorthing += 10000000.0;
560 sbg_driver::SbgEkfEuler ekf_euler_message;
563 ekf_euler_message.time_stamp = ref_log_ekf_euler.
timeStamp;
568 ekf_euler_message.angle.x = ref_log_ekf_euler.
euler[0];
569 ekf_euler_message.angle.y = -ref_log_ekf_euler.
euler[1];
574 ekf_euler_message.angle.x = ref_log_ekf_euler.
euler[0];
575 ekf_euler_message.angle.y = ref_log_ekf_euler.
euler[1];
576 ekf_euler_message.angle.z = ref_log_ekf_euler.
euler[2];
579 ekf_euler_message.accuracy.x = ref_log_ekf_euler.
eulerStdDev[0];
580 ekf_euler_message.accuracy.y = ref_log_ekf_euler.
eulerStdDev[1];
581 ekf_euler_message.accuracy.z = ref_log_ekf_euler.
eulerStdDev[2];
583 return ekf_euler_message;
588 sbg_driver::SbgEkfNav ekf_nav_message;
591 ekf_nav_message.time_stamp = ref_log_ekf_nav.
timeStamp;
593 ekf_nav_message.undulation = ref_log_ekf_nav.
undulation;
595 ekf_nav_message.latitude = ref_log_ekf_nav.
position[0];
596 ekf_nav_message.longitude = ref_log_ekf_nav.
position[1];
597 ekf_nav_message.altitude = ref_log_ekf_nav.
position[2];
601 ekf_nav_message.velocity.x = ref_log_ekf_nav.
velocity[1];
602 ekf_nav_message.velocity.y = ref_log_ekf_nav.
velocity[0];
603 ekf_nav_message.velocity.z = -ref_log_ekf_nav.
velocity[2];
605 ekf_nav_message.velocity_accuracy.x = ref_log_ekf_nav.
velocityStdDev[1];
606 ekf_nav_message.velocity_accuracy.y = ref_log_ekf_nav.
velocityStdDev[0];
607 ekf_nav_message.velocity_accuracy.z = ref_log_ekf_nav.
velocityStdDev[2];
609 ekf_nav_message.position_accuracy.x = ref_log_ekf_nav.
positionStdDev[1];
610 ekf_nav_message.position_accuracy.y = ref_log_ekf_nav.
positionStdDev[0];
611 ekf_nav_message.position_accuracy.z = ref_log_ekf_nav.
positionStdDev[2];
615 ekf_nav_message.velocity.x = ref_log_ekf_nav.
velocity[0];
616 ekf_nav_message.velocity.y = ref_log_ekf_nav.
velocity[1];
617 ekf_nav_message.velocity.z = ref_log_ekf_nav.
velocity[2];
619 ekf_nav_message.velocity_accuracy.x = ref_log_ekf_nav.
velocityStdDev[0];
620 ekf_nav_message.velocity_accuracy.y = ref_log_ekf_nav.
velocityStdDev[1];
621 ekf_nav_message.velocity_accuracy.z = ref_log_ekf_nav.
velocityStdDev[2];
623 ekf_nav_message.position_accuracy.x = ref_log_ekf_nav.
positionStdDev[0];
624 ekf_nav_message.position_accuracy.y = ref_log_ekf_nav.
positionStdDev[1];
625 ekf_nav_message.position_accuracy.z = ref_log_ekf_nav.
positionStdDev[2];
628 return ekf_nav_message;
633 sbg_driver::SbgEkfQuat ekf_quat_message;
636 ekf_quat_message.time_stamp = ref_log_ekf_quat.
timeStamp;
639 ekf_quat_message.accuracy.x = ref_log_ekf_quat.
eulerStdDev[0];
640 ekf_quat_message.accuracy.y = ref_log_ekf_quat.
eulerStdDev[1];
641 ekf_quat_message.accuracy.z = ref_log_ekf_quat.
eulerStdDev[2];
645 ekf_quat_message.quaternion.x = ref_log_ekf_quat.
quaternion[1];
646 ekf_quat_message.quaternion.y = -ref_log_ekf_quat.
quaternion[2];
647 ekf_quat_message.quaternion.z = -ref_log_ekf_quat.
quaternion[3];
648 ekf_quat_message.quaternion.w = ref_log_ekf_quat.
quaternion[0];
652 ekf_quat_message.quaternion.x = ref_log_ekf_quat.
quaternion[1];
653 ekf_quat_message.quaternion.y = ref_log_ekf_quat.
quaternion[2];
654 ekf_quat_message.quaternion.z = ref_log_ekf_quat.
quaternion[3];
655 ekf_quat_message.quaternion.w = ref_log_ekf_quat.
quaternion[0];
658 return ekf_quat_message;
663 sbg_driver::SbgEvent event_message;
666 event_message.time_stamp = ref_log_event.
timeStamp;
674 event_message.time_offset_0 = ref_log_event.
timeOffset0;
675 event_message.time_offset_1 = ref_log_event.
timeOffset1;
676 event_message.time_offset_2 = ref_log_event.
timeOffset2;
677 event_message.time_offset_3 = ref_log_event.
timeOffset3;
679 return event_message;
684 sbg_driver::SbgGpsHdt gps_hdt_message;
687 gps_hdt_message.time_stamp = ref_log_gps_hdt.
timeStamp;
688 gps_hdt_message.status = ref_log_gps_hdt.
status;
689 gps_hdt_message.tow = ref_log_gps_hdt.
timeOfWeek;
692 gps_hdt_message.baseline = ref_log_gps_hdt.
baseline;
697 gps_hdt_message.pitch = -ref_log_gps_hdt.
pitch;
701 gps_hdt_message.true_heading = ref_log_gps_hdt.
heading;
702 gps_hdt_message.pitch = ref_log_gps_hdt.
pitch;
705 return gps_hdt_message;
710 sbg_driver::SbgGpsPos gps_pos_message;
713 gps_pos_message.time_stamp = ref_log_gps_pos.
timeStamp;
716 gps_pos_message.gps_tow = ref_log_gps_pos.
timeOfWeek;
717 gps_pos_message.undulation = ref_log_gps_pos.
undulation;
718 gps_pos_message.num_sv_used = ref_log_gps_pos.
numSvUsed;
719 gps_pos_message.base_station_id = ref_log_gps_pos.
baseStationId;
722 gps_pos_message.latitude = ref_log_gps_pos.
latitude;
723 gps_pos_message.longitude = ref_log_gps_pos.
longitude;
724 gps_pos_message.altitude = ref_log_gps_pos.
altitude;
739 return gps_pos_message;
744 sbg_driver::SbgGpsRaw gps_raw_message;
748 return gps_raw_message;
753 sbg_driver::SbgGpsVel gps_vel_message;
756 gps_vel_message.time_stamp = ref_log_gps_vel.
timeStamp;
758 gps_vel_message.gps_tow = ref_log_gps_vel.
timeOfWeek;
759 gps_vel_message.course_acc = ref_log_gps_vel.
courseAcc;
763 gps_vel_message.velocity.x = ref_log_gps_vel.
velocity[1];
764 gps_vel_message.velocity.y = ref_log_gps_vel.
velocity[0];
765 gps_vel_message.velocity.z = -ref_log_gps_vel.
velocity[2];
767 gps_vel_message.velocity_accuracy.x = ref_log_gps_vel.
velocityAcc[1];
768 gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.
velocityAcc[0];
769 gps_vel_message.velocity_accuracy.z = ref_log_gps_vel.
velocityAcc[2];
775 gps_vel_message.velocity.x = ref_log_gps_vel.
velocity[0];
776 gps_vel_message.velocity.y = ref_log_gps_vel.
velocity[1];
777 gps_vel_message.velocity.z = ref_log_gps_vel.
velocity[2];
779 gps_vel_message.velocity_accuracy.x = ref_log_gps_vel.
velocityAcc[0];
780 gps_vel_message.velocity_accuracy.y = ref_log_gps_vel.
velocityAcc[1];
781 gps_vel_message.velocity_accuracy.z = ref_log_gps_vel.
velocityAcc[2];
783 gps_vel_message.course = ref_log_gps_vel.
course;
786 return gps_vel_message;
791 sbg_driver::SbgImuData imu_data_message;
794 imu_data_message.time_stamp = ref_log_imu_data.
timeStamp;
796 imu_data_message.temp = ref_log_imu_data.
temperature;
804 imu_data_message.gyro.x = ref_log_imu_data.
gyroscopes[0];
805 imu_data_message.gyro.y = -ref_log_imu_data.
gyroscopes[1];
806 imu_data_message.gyro.z = -ref_log_imu_data.
gyroscopes[2];
808 imu_data_message.delta_vel.x = ref_log_imu_data.
deltaVelocity[0];
809 imu_data_message.delta_vel.y = -ref_log_imu_data.
deltaVelocity[1];
810 imu_data_message.delta_vel.z = -ref_log_imu_data.
deltaVelocity[2];
812 imu_data_message.delta_angle.x = ref_log_imu_data.
deltaAngle[0];
813 imu_data_message.delta_angle.y = -ref_log_imu_data.
deltaAngle[1];
814 imu_data_message.delta_angle.z = -ref_log_imu_data.
deltaAngle[2];
822 imu_data_message.gyro.x = ref_log_imu_data.
gyroscopes[0];
823 imu_data_message.gyro.y = ref_log_imu_data.
gyroscopes[1];
824 imu_data_message.gyro.z = ref_log_imu_data.
gyroscopes[2];
826 imu_data_message.delta_vel.x = ref_log_imu_data.
deltaVelocity[0];
827 imu_data_message.delta_vel.y = ref_log_imu_data.
deltaVelocity[1];
828 imu_data_message.delta_vel.z = ref_log_imu_data.
deltaVelocity[2];
830 imu_data_message.delta_angle.x = ref_log_imu_data.
deltaAngle[0];
831 imu_data_message.delta_angle.y = ref_log_imu_data.
deltaAngle[1];
832 imu_data_message.delta_angle.z = ref_log_imu_data.
deltaAngle[2];
835 return imu_data_message;
840 sbg_driver::SbgMag mag_message;
843 mag_message.time_stamp = ref_log_mag.
timeStamp;
872 sbg_driver::SbgMagCalib mag_calib_message;
877 return mag_calib_message;
882 sbg_driver::SbgOdoVel odo_vel_message;
885 odo_vel_message.time_stamp = ref_log_odo.
timeStamp;
887 odo_vel_message.status = ref_log_odo.
status;
888 odo_vel_message.vel = ref_log_odo.
velocity;
890 return odo_vel_message;
895 sbg_driver::SbgShipMotion ship_motion_message;
898 ship_motion_message.time_stamp = ref_log_ship_motion.
timeStamp;
901 ship_motion_message.ship_motion.x = ref_log_ship_motion.
shipMotion[0];
902 ship_motion_message.ship_motion.y = ref_log_ship_motion.
shipMotion[1];
903 ship_motion_message.ship_motion.z = ref_log_ship_motion.
shipMotion[2];
905 ship_motion_message.acceleration.x = ref_log_ship_motion.
shipAccel[0];
906 ship_motion_message.acceleration.y = ref_log_ship_motion.
shipAccel[1];
907 ship_motion_message.acceleration.z = ref_log_ship_motion.
shipAccel[2];
909 ship_motion_message.velocity.x = ref_log_ship_motion.
shipVel[0];
910 ship_motion_message.velocity.y = ref_log_ship_motion.
shipVel[1];
911 ship_motion_message.velocity.z = ref_log_ship_motion.
shipVel[2];
913 return ship_motion_message;
918 sbg_driver::SbgStatus status_message;
921 status_message.time_stamp = ref_log_status.
timeStamp;
927 return status_message;
932 sbg_driver::SbgUtcTime utc_time_message;
935 utc_time_message.time_stamp = ref_log_utc.
timeStamp;
938 utc_time_message.year = ref_log_utc.
year;
939 utc_time_message.month = ref_log_utc.
month;
940 utc_time_message.day = ref_log_utc.
day;
941 utc_time_message.hour = ref_log_utc.
hour;
942 utc_time_message.min = ref_log_utc.
minute;
943 utc_time_message.sec = ref_log_utc.
second;
944 utc_time_message.nanosec = ref_log_utc.
nanoSecond;
949 if (utc_time_message.clock_status.clock_stable && utc_time_message.clock_status.clock_utc_sync)
954 ROS_INFO(
"A full valid UTC log has been detected, timestamp will be synchronized with the UTC data.");
964 return utc_time_message;
969 sbg_driver::SbgAirData air_data_message;
972 air_data_message.time_stamp = ref_air_data_log.
timeStamp;
974 air_data_message.pressure_abs = ref_air_data_log.
pressureAbs;
975 air_data_message.altitude = ref_air_data_log.
altitude;
976 air_data_message.pressure_diff = ref_air_data_log.
pressureDiff;
977 air_data_message.true_air_speed = ref_air_data_log.
trueAirspeed;
978 air_data_message.air_temperature = ref_air_data_log.
airTemperature;
980 return air_data_message;
985 sbg_driver::SbgImuShort imu_short_message;
988 imu_short_message.time_stamp = ref_short_imu_log.
timeStamp;
990 imu_short_message.temperature = ref_short_imu_log.
temperature;
994 imu_short_message.delta_velocity.x = ref_short_imu_log.
deltaVelocity[0];
995 imu_short_message.delta_velocity.y = -ref_short_imu_log.
deltaVelocity[1];
996 imu_short_message.delta_velocity.z = -ref_short_imu_log.
deltaVelocity[2];
998 imu_short_message.delta_angle.x = ref_short_imu_log.
deltaAngle[0];
999 imu_short_message.delta_angle.y = -ref_short_imu_log.
deltaAngle[1];
1000 imu_short_message.delta_angle.z = -ref_short_imu_log.
deltaAngle[2];
1004 imu_short_message.delta_velocity.x = ref_short_imu_log.
deltaVelocity[0];
1005 imu_short_message.delta_velocity.y = ref_short_imu_log.
deltaVelocity[1];
1006 imu_short_message.delta_velocity.z = ref_short_imu_log.
deltaVelocity[2];
1008 imu_short_message.delta_angle.x = ref_short_imu_log.
deltaAngle[0];
1009 imu_short_message.delta_angle.y = ref_short_imu_log.
deltaAngle[1];
1010 imu_short_message.delta_angle.z = ref_short_imu_log.
deltaAngle[2];
1013 return imu_short_message;
1018 sensor_msgs::Imu imu_ros_message;
1022 imu_ros_message.orientation = ref_sbg_quat_msg.quaternion;
1023 imu_ros_message.angular_velocity = ref_sbg_imu_msg.delta_angle;
1024 imu_ros_message.linear_acceleration = ref_sbg_imu_msg.delta_vel;
1026 imu_ros_message.orientation_covariance[0] = ref_sbg_quat_msg.accuracy.x * ref_sbg_quat_msg.accuracy.x;
1027 imu_ros_message.orientation_covariance[4] = ref_sbg_quat_msg.accuracy.y * ref_sbg_quat_msg.accuracy.y;
1028 imu_ros_message.orientation_covariance[8] = ref_sbg_quat_msg.accuracy.z * ref_sbg_quat_msg.accuracy.z;
1033 for (
size_t i = 0; i < 9; i++)
1035 imu_ros_message.angular_velocity_covariance[i] = 0.0;
1036 imu_ros_message.linear_acceleration_covariance[i] = 0.0;
1039 return imu_ros_message;
1042 void MessageWrapper::fillTransform(
const std::string &ref_parent_frame_id,
const std::string &ref_child_frame_id,
const geometry_msgs::Pose &ref_pose, geometry_msgs::TransformStamped &refTransformStamped)
1047 refTransformStamped.header.frame_id = ref_parent_frame_id;
1048 refTransformStamped.child_frame_id = ref_child_frame_id;
1050 refTransformStamped.transform.translation.x = ref_pose.position.x;
1051 refTransformStamped.transform.translation.y = ref_pose.position.y;
1052 refTransformStamped.transform.translation.z = ref_pose.position.z;
1053 refTransformStamped.transform.rotation.x = ref_pose.orientation.x;
1054 refTransformStamped.transform.rotation.y = ref_pose.orientation.y;
1055 refTransformStamped.transform.rotation.z = ref_pose.orientation.z;
1056 refTransformStamped.transform.rotation.w = ref_pose.orientation.w;
1061 const nav_msgs::Odometry
MessageWrapper::createRosOdoMessage(
const sbg_driver::SbgImuData &ref_sbg_imu_msg,
const sbg_driver::SbgEkfNav &ref_ekf_nav_msg,
const sbg_driver::SbgEkfQuat &ref_ekf_quat_msg,
const sbg_driver::SbgEkfEuler &ref_ekf_euler_msg)
1063 tf2::Quaternion orientation(ref_ekf_quat_msg.quaternion.x, ref_ekf_quat_msg.quaternion.y, ref_ekf_quat_msg.quaternion.z, ref_ekf_quat_msg.quaternion.w);
1065 return createRosOdoMessage(ref_sbg_imu_msg, ref_ekf_nav_msg, orientation, ref_ekf_euler_msg);
1068 const nav_msgs::Odometry
MessageWrapper::createRosOdoMessage(
const sbg_driver::SbgImuData &ref_sbg_imu_msg,
const sbg_driver::SbgEkfNav &ref_ekf_nav_msg,
const sbg_driver::SbgEkfEuler &ref_ekf_euler_msg)
1073 orientation.
setRPY(ref_ekf_euler_msg.angle.x, ref_ekf_euler_msg.angle.y, ref_ekf_euler_msg.angle.z);
1075 return createRosOdoMessage(ref_sbg_imu_msg, ref_ekf_nav_msg, orientation, ref_ekf_euler_msg);
1080 nav_msgs::Odometry odo_ros_msg;
1081 double utm_northing, utm_easting;
1082 std::string utm_zone;
1083 geometry_msgs::TransformStamped transform;
1088 tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation);
1093 initUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude);
1098 geometry_msgs::Pose pose;
1107 LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude,
m_utm0_.
zone, utm_easting, utm_northing);
1108 odo_ros_msg.pose.pose.position.x = utm_northing -
m_utm0_.
easting;
1110 odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude -
m_utm0_.
altitude;
1113 double longitudeRad =
sbgDegToRadD(ref_ekf_nav_msg.longitude);
1114 double latitudeRad =
sbgDegToRadD(ref_ekf_nav_msg.latitude);
1116 double convergence_angle =
atan(
tan(longitudeRad - central_meridian) *
sin(latitudeRad));
1119 double std_east = ref_ekf_nav_msg.position_accuracy.x;
1120 double std_north = ref_ekf_nav_msg.position_accuracy.y;
1121 double std_x = std_north *
cos(convergence_angle) - std_east *
sin(convergence_angle);
1122 double std_y = std_north * sin(convergence_angle) + std_east *
cos(convergence_angle);
1123 double std_z = ref_ekf_nav_msg.position_accuracy.z;
1124 odo_ros_msg.pose.covariance[0*6 + 0] = std_x * std_x;
1125 odo_ros_msg.pose.covariance[1*6 + 1] = std_y * std_y;
1126 odo_ros_msg.pose.covariance[2*6 + 2] = std_z * std_z;
1127 odo_ros_msg.pose.covariance[3*6 + 3] = ref_ekf_euler_msg.accuracy.x * ref_ekf_euler_msg.accuracy.x;
1128 odo_ros_msg.pose.covariance[4*6 + 4] = ref_ekf_euler_msg.accuracy.y * ref_ekf_euler_msg.accuracy.y;
1129 odo_ros_msg.pose.covariance[5*6 + 5] = ref_ekf_euler_msg.accuracy.z * ref_ekf_euler_msg.accuracy.z;
1133 odo_ros_msg.twist.twist.linear.x = ref_ekf_nav_msg.velocity.x;
1134 odo_ros_msg.twist.twist.linear.y = ref_ekf_nav_msg.velocity.y;
1135 odo_ros_msg.twist.twist.linear.z = ref_ekf_nav_msg.velocity.z;
1136 odo_ros_msg.twist.twist.angular.x = ref_sbg_imu_msg.gyro.x;
1137 odo_ros_msg.twist.twist.angular.y = ref_sbg_imu_msg.gyro.y;
1138 odo_ros_msg.twist.twist.angular.z = ref_sbg_imu_msg.gyro.z;
1139 odo_ros_msg.twist.covariance[0*6 + 0] = ref_ekf_nav_msg.velocity_accuracy.x * ref_ekf_nav_msg.velocity_accuracy.x;
1140 odo_ros_msg.twist.covariance[1*6 + 1] = ref_ekf_nav_msg.velocity_accuracy.y * ref_ekf_nav_msg.velocity_accuracy.y;
1141 odo_ros_msg.twist.covariance[2*6 + 2] = ref_ekf_nav_msg.velocity_accuracy.z * ref_ekf_nav_msg.velocity_accuracy.z;
1142 odo_ros_msg.twist.covariance[3*6 + 3] = 0;
1143 odo_ros_msg.twist.covariance[4*6 + 4] = 0;
1144 odo_ros_msg.twist.covariance[5*6 + 5] = 0;
1158 sensor_msgs::Temperature temperature_message;
1160 temperature_message.header =
createRosHeader(ref_sbg_imu_msg.time_stamp);
1161 temperature_message.temperature = ref_sbg_imu_msg.temp;
1162 temperature_message.variance = 0.0;
1164 return temperature_message;
1169 sensor_msgs::MagneticField magnetic_message;
1171 magnetic_message.header =
createRosHeader(ref_sbg_mag_msg.time_stamp);
1172 magnetic_message.magnetic_field = ref_sbg_mag_msg.mag;
1174 return magnetic_message;
1177 const geometry_msgs::TwistStamped
MessageWrapper::createRosTwistStampedMessage(
const sbg_driver::SbgEkfEuler& ref_sbg_ekf_euler_msg,
const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg,
const sbg_driver::SbgImuData& ref_sbg_imu_msg)
const 1180 tdcm.
makeDcm(
sbg::SbgVector3f(ref_sbg_ekf_euler_msg.angle.x, ref_sbg_ekf_euler_msg.angle.y, ref_sbg_ekf_euler_msg.angle.z));
1191 tdcm.
makeDcm(ref_sbg_ekf_quat_msg.quaternion.w, ref_sbg_ekf_quat_msg.quaternion.x, ref_sbg_ekf_quat_msg.quaternion.y, ref_sbg_ekf_quat_msg.quaternion.z);
1200 geometry_msgs::TwistStamped twist_stamped_message;
1202 twist_stamped_message.header =
createRosHeader(ref_sbg_imu_msg.time_stamp);
1203 twist_stamped_message.twist.angular = ref_sbg_imu_msg.delta_angle;
1205 twist_stamped_message.twist.linear.x = body_vel(0);
1206 twist_stamped_message.twist.linear.y = body_vel(1);
1207 twist_stamped_message.twist.linear.z = body_vel(2);
1209 return twist_stamped_message;
1214 geometry_msgs::PointStamped point_stamped_message;
1216 point_stamped_message.header =
createRosHeader(ref_sbg_ekf_msg.time_stamp);
1222 double equatorial_radius;
1223 double polar_radius;
1224 double prime_vertical_radius;
1225 double eccentricity;
1229 equatorial_radius = 6378137.0;
1230 polar_radius = 6356752.314245;
1231 eccentricity = 1 -
pow(polar_radius, 2) /
pow(equatorial_radius, 2);
1235 prime_vertical_radius = equatorial_radius /
sqrt(1.0 -
pow(eccentricity, 2) *
pow(
sin(latitude), 2));
1237 point_stamped_message.point.x = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) *
cos(latitude) *
cos(longitude);
1238 point_stamped_message.point.y = (prime_vertical_radius + ref_sbg_ekf_msg.altitude) *
cos(latitude) *
sin(longitude);
1239 point_stamped_message.point.z = ((
pow(polar_radius, 2) /
pow(equatorial_radius, 2)) * prime_vertical_radius + ref_sbg_ekf_msg.altitude) *
sin(latitude);
1241 return point_stamped_message;
1254 utc_reference_message.source =
"UTC time from device converted to Epoch";
1256 return utc_reference_message;
1261 sensor_msgs::NavSatFix nav_sat_fix_message;
1263 nav_sat_fix_message.header =
createRosHeader(ref_sbg_gps_msg.time_stamp);
1267 nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_NO_FIX;
1271 nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_SBAS_FIX;
1275 nav_sat_fix_message.status.status = nav_sat_fix_message.status.STATUS_FIX;
1278 if (ref_sbg_gps_msg.status.glo_l1_used || ref_sbg_gps_msg.status.glo_l2_used)
1280 nav_sat_fix_message.status.service = nav_sat_fix_message.status.SERVICE_GLONASS;
1284 nav_sat_fix_message.status.service = nav_sat_fix_message.status.SERVICE_GPS;
1287 nav_sat_fix_message.latitude = ref_sbg_gps_msg.latitude;
1288 nav_sat_fix_message.longitude = ref_sbg_gps_msg.longitude;
1289 nav_sat_fix_message.altitude = ref_sbg_gps_msg.altitude + ref_sbg_gps_msg.undulation;
1291 nav_sat_fix_message.position_covariance[0] = ref_sbg_gps_msg.position_accuracy.x * ref_sbg_gps_msg.position_accuracy.x;
1292 nav_sat_fix_message.position_covariance[4] = ref_sbg_gps_msg.position_accuracy.y * ref_sbg_gps_msg.position_accuracy.y;
1293 nav_sat_fix_message.position_covariance[8] = ref_sbg_gps_msg.position_accuracy.z * ref_sbg_gps_msg.position_accuracy.z;
1295 nav_sat_fix_message.position_covariance_type = nav_sat_fix_message.COVARIANCE_TYPE_DIAGONAL_KNOWN;
1297 return nav_sat_fix_message;
1302 sensor_msgs::FluidPressure fluid_pressure_message;
1304 fluid_pressure_message.header =
createRosHeader(ref_sbg_air_msg.time_stamp);
1305 fluid_pressure_message.fluid_pressure = ref_sbg_air_msg.pressure_abs;
1306 fluid_pressure_message.variance = 0.0;
1308 return fluid_pressure_message;
#define SBG_ECOM_PORTD_RX_OK
bool isLeapYear(uint16_t year) const
#define SBG_ECOM_AIDING_GPS1_VEL_RECV
#define SBG_ECOM_PORTD_TX_OK
#define SBG_ECOM_GPS_POS_GLO_L1_USED
#define SBG_ECOM_CLOCK_STABLE_INPUT
const sensor_msgs::NavSatFix createRosNavSatFixMessage(const sbg_driver::SbgGpsPos &ref_sbg_gps_msg) const
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
tf2_ros::TransformBroadcaster m_tf_broadcaster_
#define SBG_ECOM_GENERAL_MAIN_POWER_OK
const sbg_driver::SbgImuShort createSbgImuShortMessage(const SbgLogImuShort &ref_short_imu_log) const
SBG_INLINE SbgEComGpsVelType sbgEComLogGpsVelGetType(uint32_t status)
const std_msgs::Header createRosHeader(uint32_t device_timestamp) const
const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const sbg_driver::SbgEkfQuat &ref_sbg_ekf_quat_msg, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg)
void setOdomEnable(bool odom_enable)
#define SBG_ECOM_AIDING_MAG_RECV
uint32_t getNumberOfDaysInYear(uint16_t year) const
const sbg_driver::SbgEkfNav createSbgEkfNavMessage(const SbgLogEkfNavData &ref_log_ekf_nav) const
const sbg_driver::SbgAirData createSbgAirDataMessage(const SbgLogAirData &ref_air_data_log) const
#define SBG_ECOM_SOL_GPS1_POS_USED
#define SBG_ECOM_GPS_POS_GPS_L5_USED
#define SBG_ECOM_IMU_GYRO_Y_BIT
#define SBG_ECOM_EVENT_OFFSET_2_VALID
const sbg_driver::SbgStatusCom createStatusComMessage(const SbgLogStatusData &ref_log_status) const
const sensor_msgs::TimeReference createRosUtcTimeReferenceMessage(const sbg_driver::SbgUtcTime &ref_sbg_utc_msg) const
#define SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID
#define SBG_ECOM_SOL_MAG_REF_USED
#define SBG_ECOM_PORTA_VALID
Time & fromNSec(uint64_t t)
#define SBG_ECOM_IMU_GYRO_Z_BIT
sbg_driver::SbgUtcTime m_last_sbg_utc_
#define SBG_ECOM_MAG_CALIBRATION_OK
#define SBG_ECOM_MAG_MAG_Z_BIT
#define SBG_ECOM_EVENT_OVERFLOW
#define SBG_ECOM_EVENT_OFFSET_1_VALID
void setOdomPublishTf(bool publish_tf)
#define SBG_ECOM_SOL_VERT_REF_USED
#define SBG_ECOM_SOL_HEADING_VALID
#define SBG_ECOM_SOL_GPS2_VEL_USED
SBG_INLINE SbgEComGpsPosStatus sbgEComLogGpsPosGetStatus(uint32_t status)
const sbg_driver::SbgImuStatus createImuStatusMessage(uint16_t sbg_imu_status) const
#define SBG_ECOM_SOL_GPS2_POS_USED
const sbg_driver::SbgAirDataStatus createAirDataStatusMessage(const SbgLogAirData &ref_sbg_air_data) const
#define SBG_ECOM_AIDING_DVL_RECV
const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f &body_vel, const sbg_driver::SbgImuData &ref_sbg_imu_msg) const
const sbg_driver::SbgUtcTimeStatus createUtcStatusMessage(const SbgLogUtcData &ref_log_utc) const
const sbg_driver::SbgEkfStatus createEkfStatusMessage(uint32_t ekf_status) const
std::string m_odom_frame_id_
const ros::Time convertUtcToUnix(const sbg_driver::SbgUtcTime &ref_sbg_utc_msg) const
#define SBG_ECOM_AIR_DATA_AIRPSEED_VALID
#define SBG_ECOM_MAG_ACCEL_Y_BIT
#define SBG_ECOM_PORTB_TX_OK
#define SBG_ECOM_PORTE_TX_OK
#define SBG_ECOM_PORTB_VALID
const sbg_driver::SbgStatusGeneral createStatusGeneralMessage(const SbgLogStatusData &ref_log_status) const
#define SBG_ECOM_SOL_POSITION_VALID
#define SBG_ECOM_PORTC_VALID
#define SBG_ECOM_PORTD_VALID
uint8_t rawBuffer[SBG_ECOM_GPS_RAW_MAX_BUFFER_SIZE]
#define SBG_ECOM_HEAVE_PERIOD_INCLUDED
Handle creation of messages.
#define SBG_ECOM_SOL_ODO_USED
#define SBG_ECOM_GENERAL_IMU_POWER_OK
SBG_INLINE SbgEComGpsPosType sbgEComLogGpsPosGetType(uint32_t status)
#define SBG_ECOM_MAG_MAG_Y_BIT
#define SBG_ECOM_IMU_ACCEL_X_BIT
const sbg_driver::SbgEkfQuat createSbgEkfQuatMessage(const SbgLogEkfQuatData &ref_log_ekf_quat) const
SBG_INLINE SbgEComClockStatus sbgEComLogUtcGetClockStatus(uint16_t status)
const sbg_driver::SbgGpsPos createSbgGpsPosMessage(const SbgLogGpsPos &ref_log_gps_pos) const
#define SBG_ECOM_AIR_DATA_ALTITUDE_VALID
float wrapAngle360(float angle_deg) const
SBG_INLINE SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status)
void setFrameId(const std::string &frame_id)
#define SBG_ECOM_GPS_POS_GPS_L2_USED
#define SBG_ECOM_AIDING_ODO_RECV
void setTimeReference(TimeReference time_reference)
#define SBG_ECOM_MAG_MAG_X_BIT
#define SBG_ECOM_IMU_ACCELS_IN_RANGE
const sbg_driver::SbgUtcTime createSbgUtcTimeMessage(const SbgLogUtcData &ref_log_utc)
uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const
const sensor_msgs::FluidPressure createRosFluidPressureMessage(const sbg_driver::SbgAirData &ref_sbg_air_msg) const
#define SBG_ECOM_CAN_VALID
#define SBG_ECOM_MAG_ACCEL_X_BIT
#define SBG_ECOM_PORTC_TX_OK
#define SBG_ECOM_GPS_POS_GLO_L2_USED
double computeMeridian(int zone_number) const
float wrapAngle2Pi(float angle_rad) const
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
const sbg_driver::SbgShipMotion createSbgShipMotionMessage(const SbgLogShipMotionData &ref_log_ship_motion) const
#define SBG_ECOM_PORTB_RX_OK
#define SBG_ECOM_CLOCK_UTC_SYNC
#define SBG_ECOM_MAG_MAGS_IN_RANGE
const sbg_driver::SbgMagCalib createSbgMagCalibMessage(const SbgLogMagCalib &ref_log_mag_calib) const
const sbg_driver::SbgEvent createSbgEventMessage(const SbgLogEvent &ref_log_event) const
const ros::Time convertInsTimeToUnix(uint32_t device_timestamp) const
SBG_INLINE SbgEComClockUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status)
#define SBG_ECOM_CAN_RX_OK
SbgVector3< float > SbgVector3f
TimeReference m_time_reference_
SBG_INLINE double sbgDegToRadD(double angle)
#define SBG_ECOM_PORTE_VALID
std::string m_odom_init_frame_id_
#define SBG_ECOM_SOL_GPS1_VEL_USED
#define SBG_ECOM_GPS_POS_GPS_L1_USED
const sensor_msgs::Imu createRosImuMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat &ref_sbg_quat_msg) const
#define SBG_ECOM_SOL_VELOCITY_VALID
const sbg_driver::SbgStatus createSbgStatusMessage(const SbgLogStatusData &ref_log_status) const
#define SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
SBG_INLINE SbgEComGpsVelStatus sbgEComLogGpsVelGetStatus(uint32_t status)
#define SBG_ECOM_MAG_ACCEL_Z_BIT
const sbg_driver::SbgMagStatus createMagStatusMessage(const SbgLogMag &ref_log_mag) const
#define SBG_ECOM_GENERAL_SETTINGS_OK
#define SBG_ECOM_SOL_GPS2_HDT_USED
const sbg_driver::SbgShipMotionStatus createShipMotionStatusMessage(const SbgLogShipMotionData &ref_log_ship_motion) const
#define SBG_ECOM_CAN_TX_OK
void setOdomBaseFrameId(const std::string &ref_frame_id)
#define SBG_ECOM_HEAVE_PERIOD_VALID
#define SBG_ECOM_AIDING_GPS1_UTC_RECV
const geometry_msgs::PointStamped createRosPointStampedMessage(const sbg_driver::SbgEkfNav &ref_sbg_ekf_msg) const
#define SBG_ECOM_EVENT_OFFSET_0_VALID
#define SBG_ECOM_AIR_DATA_TIME_IS_DELAY
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
#define SBG_ECOM_MAG_ACCELS_IN_RANGE
#define SBG_ECOM_AIDING_GPS1_POS_RECV
const sbg_driver::SbgStatusAiding createStatusAidingMessage(const SbgLogStatusData &ref_log_status) const
#define SBG_ECOM_GENERAL_TEMPERATURE_OK
const sbg_driver::SbgImuData createSbgImuDataMessage(const SbgLogImuData &ref_log_imu_data) const
#define SBG_ECOM_PORTE_RX_OK
#define SBG_ECOM_HEAVE_VALID
char UTMLetterDesignator(double Lat)
const sbg_driver::SbgEkfEuler createSbgEkfEulerMessage(const SbgLogEkfEulerData &ref_log_ekf_euler) const
void setOdomFrameId(const std::string &ref_frame_id)
void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::Pose &ref_pose, geometry_msgs::TransformStamped &ref_transform_stamped)
#define SBG_ECOM_SOL_GPS1_HDT_USED
void makeDcm(const SbgVector3f &euler)
const sbg_driver::SbgGpsVel createSbgGpsVelMessage(const SbgLogGpsVel &ref_log_gps_vel) const
#define SBG_ECOM_AIDING_GPS1_HDT_RECV
#define SBG_ECOM_IMU_ACCEL_Z_BIT
void convert(const A &a, B &b)
const sensor_msgs::Temperature createRosTemperatureMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg) const
#define SBG_ECOM_AIR_DATA_TEMPERATURE_VALID
const sbg_driver::SbgGpsVelStatus createGpsVelStatusMessage(const SbgLogGpsVel &ref_log_gps_vel) const
const sbg_driver::SbgMag createSbgMagMessage(const SbgLogMag &ref_log_mag) const
#define SBG_ECOM_GENERAL_GPS_POWER_OK
#define SBG_ECOM_PORTA_RX_OK
const sensor_msgs::MagneticField createRosMagneticMessage(const sbg_driver::SbgMag &ref_sbg_mag_msg) const
#define SBG_ECOM_IMU_GYROS_IN_RANGE
#define SBG_ECOM_PORTC_RX_OK
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
#define SBG_ECOM_PORTA_TX_OK
void setOdomInitFrameId(const std::string &ref_frame_id)
Handle a three components vector.
void initUTM(double Lat, double Long, double altitude)
#define SBG_ECOM_SOL_ATTITUDE_VALID
#define SBG_ECOM_EVENT_OFFSET_3_VALID
void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const
#define SBG_ECOM_IMU_GYRO_X_BIT
const sbg_driver::SbgGpsPosStatus createGpsPosStatusMessage(const SbgLogGpsPos &ref_log_gps_pos) const
const sbg_driver::SbgOdoVel createSbgOdoVelMessage(const SbgLogOdometerData &ref_log_odo) const
#define SBG_ECOM_IMU_COM_OK
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
#define SBG_ECOM_IMU_ACCEL_Y_BIT
const sbg_driver::SbgGpsHdt createSbgGpsHdtMessage(const SbgLogGpsHdt &ref_log_gps_hdt) const
std::string m_odom_base_frame_id_
tf2_ros::StaticTransformBroadcaster m_static_tf_broadcaster_
const sbg_driver::SbgGpsRaw createSbgGpsRawMessage(const SbgLogGpsRaw &ref_log_gps_raw) const
#define SBG_ECOM_IMU_STATUS_BIT
#define SBG_ECOM_HEAVE_VEL_AIDED