My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
holistic_fusion::HolisticFusionRos Class Reference

#include <HolisticFusionRos.h>

Inheritance diagram for holistic_fusion::HolisticFusionRos:
Inheritance graph
[legend]
Collaboration diagram for holistic_fusion::HolisticFusionRos:
Collaboration graph
[legend]

Public Member Functions

 HolisticFusionRos (const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
 ~HolisticFusionRos () override=default
void setup (const std::shared_ptr< StaticTransforms > staticTransformsPtr)
Public Member Functions inherited from holistic_fusion::HolisticFusionClassic
 HolisticFusionClassic ()
virtual ~HolisticFusionClassic ()=default
void addUnaryRollAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &roll_F_S) override
 Unary Measurements / Absolute Measurements.
void addUnaryPitchAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &pitch_F_S) override
void addUnaryYawAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S) override
void addBinaryPose3Measurement (const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &F_T_F_S) override
 Binary Measurements.
Public Member Functions inherited from holistic_fusion::HolisticFusion
 HolisticFusion ()
 Constructor --------—.
virtual ~HolisticFusion ()
 Destructor --------—.
void setup (const std::shared_ptr< GraphConfig > graphConfigPtr, const std::shared_ptr< StaticTransforms > staticTransformsPtr)
bool initYawAndPositionInWorld (const double yaw_W_frame1, const Eigen::Vector3d &W_t_W_frame2, const std::string &frame1, const std::string &frame2)
bool initYawAndPosition (const UnaryMeasurementXD< Eigen::Isometry3d, 6 > &unary6DMeasurement)
bool initWorldFrameToFixedFrameTransform (const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
bool optimizeSlowBatchSmoother (int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
std::optional< BatchOptimizationResult > optimizeSlowBatchSmoother (int maxIterations, const bool computePoseCovarianceFlag)
bool logRealTimeNavStates (const std::string &savePath)
bool areYawAndPositionInited () const
bool areRollAndPitchInited () const
bool isGraphInited () const
bool getNormalOperationFlag () const
bool addCoreImuMeasurementAndGetState (const Eigen::Vector3d &linearAcc, const Eigen::Vector3d &angularVel, const double imuTimeK, std::shared_ptr< SafeIntegratedNavState > &returnPreIntegratedNavStatePtr, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &returnOptimizedStateWithCovarianceAndBiasPtr, Eigen::Matrix< double, 6, 1 > &returnAddedImuMeasurements)
 Main: IMU --------------------—.
bool addZeroMotionFactor (double timeKm1, double timeK, double noiseDensity)
bool addZeroVelocityFactor (double timeK, double noiseDensity)
Public Member Functions inherited from holistic_fusion::HolisticFusionHolistic
 HolisticFusionHolistic ()
virtual ~HolisticFusionHolistic ()=default
void addUnaryPose3AbsoluteMeasurement (const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &R_T_R_S, const bool addToOnlineSmootherFlag=true) override
 Unary Measurements / Absolute Measurements: In reference frame --> systematic drift.
void addUnaryPosition3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_t_R_S) override
void addUnaryHeadingAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< Eigen::Matrix3d, 1 > &R_M_S)
void addUnaryVelocity3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_v_R_S) override
void addUnaryVelocity3LocalMeasurement (UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S) override
void addUnaryVelocity3LocalMovingFrameMeasurement (UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S, const Eigen::Isometry3d &T_I_sensorFrame, const Eigen::Vector3d &I_w_W_I)
void addUnaryPosition3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_t_S_L, const int landmarkCreationCounter) override
 Landmark Measurements: No systematic drift.
void addUnaryBearing3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_bearing_S_L) override

Protected Member Functions

virtual void initializePublishers (ros::NodeHandle &privateNode)
virtual void initializeSubscribers (ros::NodeHandle &privateNode)
virtual void initializeMessages (ros::NodeHandle &privateNode)
virtual void initializeServices (ros::NodeHandle &privateNode)
virtual void readParams (const ros::NodeHandle &privateNode)
virtual void imuCallback (const sensor_msgs::Imu::ConstPtr &imuPtr)
virtual bool srvOfflineSmootherOptimizeCallback (holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res)
bool srvLogRealTimeNavStatesCallback (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
virtual void publishState (const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > &integratedNavStatePtr, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr)
void publishNonTimeCriticalData (const Eigen::Matrix< double, 6, 6 > poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > twistCovarianceRos, const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > integratedNavStatePtr, const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr)
void publishOptimizedStateAndBias (const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr, const Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > &twistCovarianceRos)
void publishTfTreeTransform (const std::string &frameName, const std::string &childFrameName, double timeStamp, const Eigen::Isometry3d &T_frame_childFrame)
void publishImuOdoms (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &preIntegratedNavStatePtr, const Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > &twistCovarianceRos) const
void publishDiagVarianceVectors (const Eigen::Vector3d &posVarianceRos, const Eigen::Vector3d &rotVarianceRos, const double timeStamp) const
void publishVelocityMarkers (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &navStatePtr) const
void publishImuPaths (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &navStatePtr) const
void publishAddedImuMeas (const Eigen::Matrix< double, 6, 1 > &addedImuMeas, const ros::Time &stamp) const
long secondsSinceStart ()
Protected Member Functions inherited from holistic_fusion::HolisticFusion
template<int DIM>
bool isCovarianceViolated_ (const Eigen::Matrix< double, DIM, 1 > &covariance, const double covarianceViolationThreshold)
 Convenience functions.
void pretendFirstMeasurementReceived ()
bool checkAndPrintCovarianceViolation_ (const std::string &measurementName, const bool violatedFlag)

Static Protected Member Functions

static void addToPathMsg (const nav_msgs::PathPtr &pathPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Vector3d &t, int maxBufferLength)
static void addToOdometryMsg (const nav_msgs::OdometryPtr &msgPtr, const std::string &fixedFrame, const std::string &movingFrame, const ros::Time &stamp, const Eigen::Isometry3d &T, const Eigen::Vector3d &W_v_W_F, const Eigen::Vector3d &W_w_W_F, const Eigen::Matrix< double, 6, 6 > &poseCovariance=Eigen::Matrix< double, 6, 6 >::Zero(), const Eigen::Matrix< double, 6, 6 > &twistCovariance=Eigen::Matrix< double, 6, 6 >::Zero())
static void addToPoseWithCovarianceStampedMsg (const geometry_msgs::PoseWithCovarianceStampedPtr &msgPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Isometry3d &T, const Eigen::Matrix< double, 6, 6 > &transformCovariance=Eigen::Matrix< double, 6, 6 >::Zero())
static void addToLandmarkMarkerArrayMsg (const visualization_msgs::MarkerArrayPtr &markerArrayPtr, const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Isometry3d &T_R_L, const std::string &transformName)
static void extractCovariancesFromOptimizedState (Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, Eigen::Matrix< double, 6, 6 > &twistCovarianceRos, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr)
static void createVelocityMarker (const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Vector3d &velocity, const Eigen::Vector3d &colorRgb, visualization_msgs::Marker &marker)
static void createContactMarker (const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Vector3d &position, const std::string &nameSpace, const int id, const Eigen::Vector3d &colorRgb, visualization_msgs::Marker &marker)

Protected Attributes

ros::NodeHandle privateNode
std::chrono::time_point< std::chrono::high_resolution_clock > startTime
std::chrono::time_point< std::chrono::high_resolution_clock > currentTime
tf::TransformBroadcaster tfBroadcaster
std::string referenceFrameAlignedNameId = "_holistic_fusion_aligned"
std::string sensorFrameCorrectedNameId = "_holistic_fusion_corrected"
std::string optimizationResultLoggingPath = ""
Protected Attributes inherited from holistic_fusion::HolisticFusion
std::shared_ptr< GraphConfig > graphConfigPtr_ = nullptr
std::shared_ptr< StaticTransforms > staticTransformsPtr_ = nullptr
std::shared_ptr< GraphManager > graphMgrPtr_ = nullptr
std::shared_ptr< holistic_fusion::ImuBuffer > coreImuBufferPtr_
std::mutex optimizeGraphMutex_
bool alignedImuFlag_ = false
 Flags / Initialization.
bool foundInitialYawAndPositionFlag_ = false
bool initedGraphFlag_ = false
bool validFirstMeasurementReceivedFlag_ = false
bool optimizeGraphFlag_ = false
bool normalOperationFlag_ = false
long imuCallbackCounter_ = 0
std::unordered_set< std::string > measurementsWithViolatedCovariance_

Private Attributes

ros::Publisher pubEstOdomImu_
ros::Publisher pubEstWorldImu_
ros::Publisher pubOptWorldImu_
ros::Publisher pubEstWorldPosVariance_
ros::Publisher pubEstWorldRotVariance_
ros::Publisher pubVelocityMarker_
ros::Publisher pubEstOdomImuPath_
ros::Publisher pubEstWorldImuPath_
ros::Publisher pubOptWorldImuPath_
ros::Publisher pubMeasWorldGnssLPath_
ros::Publisher pubMeasWorldGnssRPath_
ros::Publisher pubMeasWorldLidarPath_
ros::Publisher pubAccelBias_
ros::Publisher pubGyroBias_
ros::Publisher pubAddedImuMeas_
std::map< std::string, ros::Publisher > pubPoseStampedByTopicMap_ = {}
ros::Publisher pubLandmarks_
ros::Subscriber subImu_
nav_msgs::OdometryPtr estOdomImuMsgPtr_
nav_msgs::OdometryPtr estWorldImuMsgPtr_
nav_msgs::OdometryPtr optWorldImuMsgPtr_
geometry_msgs::Vector3StampedPtr estWorldPosVarianceMsgPtr_
geometry_msgs::Vector3StampedPtr estWorldRotVarianceMsgPtr_
nav_msgs::PathPtr estOdomImuPathPtr_
nav_msgs::PathPtr estWorldImuPathPtr_
nav_msgs::PathPtr optWorldImuPathPtr_
nav_msgs::PathPtr measWorldLidarPathPtr_
geometry_msgs::Vector3StampedPtr accelBiasMsgPtr_
geometry_msgs::Vector3StampedPtr gyroBiasMsgPtr_
visualization_msgs::MarkerArrayPtr landmarkMarkersMsgPtr_
ros::ServiceServer srvSmootherOptimize_
ros::ServiceServer srvLogRealTimeNavStates_
double lastOptimizedStateTimestamp_ = 0.0
std::mutex rosPublisherMutex_

Detailed Description

Definition at line 38 of file HolisticFusionRos.h.

Constructor & Destructor Documentation

◆ HolisticFusionRos()

holistic_fusion::HolisticFusionRos::HolisticFusionRos ( const std::shared_ptr< ros::NodeHandle > & privateNodePtr)
explicit

Definition at line 21 of file HolisticFusionRos.cpp.

◆ ~HolisticFusionRos()

holistic_fusion::HolisticFusionRos::~HolisticFusionRos ( )
overridedefault

Member Function Documentation

◆ addToLandmarkMarkerArrayMsg()

void holistic_fusion::HolisticFusionRos::addToLandmarkMarkerArrayMsg ( const visualization_msgs::MarkerArrayPtr & markerArrayPtr,
const std::string & referenceFrameName,
const ros::Time & stamp,
const Eigen::Isometry3d & T_R_L,
const std::string & transformName )
staticprotected

Definition at line 205 of file HolisticFusionRos.cpp.

◆ addToOdometryMsg()

void holistic_fusion::HolisticFusionRos::addToOdometryMsg ( const nav_msgs::OdometryPtr & msgPtr,
const std::string & fixedFrame,
const std::string & movingFrame,
const ros::Time & stamp,
const Eigen::Isometry3d & T,
const Eigen::Vector3d & W_v_W_F,
const Eigen::Vector3d & W_w_W_F,
const Eigen::Matrix< double, 6, 6 > & poseCovariance = Eigen::Matrix<double, 6, 6>::Zero(),
const Eigen::Matrix< double, 6, 6 > & twistCovariance = Eigen::Matrix<double, 6, 6>::Zero() )
staticprotected

Definition at line 169 of file HolisticFusionRos.cpp.

◆ addToPathMsg()

void holistic_fusion::HolisticFusionRos::addToPathMsg ( const nav_msgs::PathPtr & pathPtr,
const std::string & frameName,
const ros::Time & stamp,
const Eigen::Vector3d & t,
int maxBufferLength )
staticprotected

Definition at line 153 of file HolisticFusionRos.cpp.

◆ addToPoseWithCovarianceStampedMsg()

void holistic_fusion::HolisticFusionRos::addToPoseWithCovarianceStampedMsg ( const geometry_msgs::PoseWithCovarianceStampedPtr & msgPtr,
const std::string & frameName,
const ros::Time & stamp,
const Eigen::Isometry3d & T,
const Eigen::Matrix< double, 6, 6 > & transformCovariance = Eigen::Matrix<double, 6, 6>::Zero() )
staticprotected

Definition at line 192 of file HolisticFusionRos.cpp.

◆ createContactMarker()

void holistic_fusion::HolisticFusionRos::createContactMarker ( const std::string & referenceFrameName,
const ros::Time & stamp,
const Eigen::Vector3d & position,
const std::string & nameSpace,
const int id,
const Eigen::Vector3d & colorRgb,
visualization_msgs::Marker & marker )
staticprotected

Definition at line 285 of file HolisticFusionRos.cpp.

◆ createVelocityMarker()

void holistic_fusion::HolisticFusionRos::createVelocityMarker ( const std::string & referenceFrameName,
const ros::Time & stamp,
const Eigen::Vector3d & velocity,
const Eigen::Vector3d & colorRgb,
visualization_msgs::Marker & marker )
staticprotected

Definition at line 251 of file HolisticFusionRos.cpp.

◆ extractCovariancesFromOptimizedState()

void holistic_fusion::HolisticFusionRos::extractCovariancesFromOptimizedState ( Eigen::Matrix< double, 6, 6 > & poseCovarianceRos,
Eigen::Matrix< double, 6, 6 > & twistCovarianceRos,
const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > & optimizedStateWithCovarianceAndBiasPtr )
staticprotected

Definition at line 236 of file HolisticFusionRos.cpp.

◆ imuCallback()

void holistic_fusion::HolisticFusionRos::imuCallback ( const sensor_msgs::Imu::ConstPtr & imuPtr)
protectedvirtual

Reimplemented in imu_integrator::ImuIntegrator, and smb_se::SmbEstimator.

Definition at line 320 of file HolisticFusionRos.cpp.

◆ initializeMessages()

void holistic_fusion::HolisticFusionRos::initializeMessages ( ros::NodeHandle & privateNode)
protectedvirtual

Reimplemented in anymal_se::AnymalEstimator, excavator_se::ExcavatorEstimator, imu_pose3_fuser::ImuPose3Fuser, position3_se::Position3Estimator, and smb_se::SmbEstimator.

Definition at line 95 of file HolisticFusionRos.cpp.

◆ initializePublishers()

void holistic_fusion::HolisticFusionRos::initializePublishers ( ros::NodeHandle & privateNode)
protectedvirtual

Reimplemented in anymal_se::AnymalEstimator, excavator_se::ExcavatorEstimator, imu_pose3_fuser::ImuPose3Fuser, position3_se::Position3Estimator, and smb_se::SmbEstimator.

Definition at line 62 of file HolisticFusionRos.cpp.

◆ initializeServices()

void holistic_fusion::HolisticFusionRos::initializeServices ( ros::NodeHandle & privateNode)
protectedvirtual

Reimplemented in imu_pose3_fuser::ImuPose3Fuser, position3_se::Position3Estimator, and smb_se::SmbEstimator.

Definition at line 115 of file HolisticFusionRos.cpp.

◆ initializeSubscribers()

void holistic_fusion::HolisticFusionRos::initializeSubscribers ( ros::NodeHandle & privateNode)
protectedvirtual

Reimplemented in anymal_se::AnymalEstimator, excavator_se::ExcavatorEstimator, imu_pose3_fuser::ImuPose3Fuser, position3_se::Position3Estimator, and smb_se::SmbEstimator.

Definition at line 86 of file HolisticFusionRos.cpp.

◆ publishAddedImuMeas()

void holistic_fusion::HolisticFusionRos::publishAddedImuMeas ( const Eigen::Matrix< double, 6, 1 > & addedImuMeas,
const ros::Time & stamp ) const
protected

Definition at line 635 of file HolisticFusionRos.cpp.

◆ publishDiagVarianceVectors()

void holistic_fusion::HolisticFusionRos::publishDiagVarianceVectors ( const Eigen::Vector3d & posVarianceRos,
const Eigen::Vector3d & rotVarianceRos,
const double timeStamp ) const
protected

Definition at line 585 of file HolisticFusionRos.cpp.

◆ publishImuOdoms()

void holistic_fusion::HolisticFusionRos::publishImuOdoms ( const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > & preIntegratedNavStatePtr,
const Eigen::Matrix< double, 6, 6 > & poseCovarianceRos,
const Eigen::Matrix< double, 6, 6 > & twistCovarianceRos ) const
protected

Definition at line 566 of file HolisticFusionRos.cpp.

◆ publishImuPaths()

void holistic_fusion::HolisticFusionRos::publishImuPaths ( const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > & navStatePtr) const
protected

Definition at line 620 of file HolisticFusionRos.cpp.

◆ publishNonTimeCriticalData()

void holistic_fusion::HolisticFusionRos::publishNonTimeCriticalData ( const Eigen::Matrix< double, 6, 6 > poseCovarianceRos,
const Eigen::Matrix< double, 6, 6 > twistCovarianceRos,
const Eigen::Vector3d positionVarianceRos,
const Eigen::Vector3d orientationVarianceRos,
const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > integratedNavStatePtr,
const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr )
protected

Definition at line 370 of file HolisticFusionRos.cpp.

◆ publishOptimizedStateAndBias()

void holistic_fusion::HolisticFusionRos::publishOptimizedStateAndBias ( const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr,
const Eigen::Matrix< double, 6, 6 > & poseCovarianceRos,
const Eigen::Matrix< double, 6, 6 > & twistCovarianceRos )
protected

Definition at line 407 of file HolisticFusionRos.cpp.

◆ publishState()

void holistic_fusion::HolisticFusionRos::publishState ( const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > & integratedNavStatePtr,
const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > & optimizedStateWithCovarianceAndBiasPtr )
protectedvirtual

Definition at line 351 of file HolisticFusionRos.cpp.

◆ publishTfTreeTransform()

void holistic_fusion::HolisticFusionRos::publishTfTreeTransform ( const std::string & frameName,
const std::string & childFrameName,
double timeStamp,
const Eigen::Isometry3d & T_frame_childFrame )
protected

Definition at line 560 of file HolisticFusionRos.cpp.

◆ publishVelocityMarkers()

void holistic_fusion::HolisticFusionRos::publishVelocityMarkers ( const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > & navStatePtr) const
protected

Definition at line 607 of file HolisticFusionRos.cpp.

◆ readParams()

void holistic_fusion::HolisticFusionRos::readParams ( const ros::NodeHandle & privateNode)
protectedvirtual

IMU / Position

Thresholds

Others

World

Odom

IMU / Cabin IMU

Reimplemented in anymal_se::AnymalEstimator, excavator_se::ExcavatorEstimator, imu_pose3_fuser::ImuPose3Fuser, position3_se::Position3Estimator, and smb_se::SmbEstimator.

Definition at line 19 of file readParams.cpp.

◆ secondsSinceStart()

long holistic_fusion::HolisticFusionRos::secondsSinceStart ( )
protected

Definition at line 314 of file HolisticFusionRos.cpp.

◆ setup()

void holistic_fusion::HolisticFusionRos::setup ( const std::shared_ptr< StaticTransforms > staticTransformsPtr)

Definition at line 29 of file HolisticFusionRos.cpp.

◆ srvLogRealTimeNavStatesCallback()

bool holistic_fusion::HolisticFusionRos::srvLogRealTimeNavStatesCallback ( std_srvs::Trigger::Request & req,
std_srvs::Trigger::Response & res )
protected

Definition at line 141 of file HolisticFusionRos.cpp.

◆ srvOfflineSmootherOptimizeCallback()

bool holistic_fusion::HolisticFusionRos::srvOfflineSmootherOptimizeCallback ( holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request & req,
holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response & res )
protectedvirtual

Reimplemented in anymal_se::AnymalEstimator, and position3_se::Position3Estimator.

Definition at line 124 of file HolisticFusionRos.cpp.

Member Data Documentation

◆ accelBiasMsgPtr_

geometry_msgs::Vector3StampedPtr holistic_fusion::HolisticFusionRos::accelBiasMsgPtr_
private

Definition at line 185 of file HolisticFusionRos.h.

◆ currentTime

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::HolisticFusionRos::currentTime
protected

Definition at line 125 of file HolisticFusionRos.h.

◆ estOdomImuMsgPtr_

nav_msgs::OdometryPtr holistic_fusion::HolisticFusionRos::estOdomImuMsgPtr_
private

Definition at line 171 of file HolisticFusionRos.h.

◆ estOdomImuPathPtr_

nav_msgs::PathPtr holistic_fusion::HolisticFusionRos::estOdomImuPathPtr_
private

Definition at line 179 of file HolisticFusionRos.h.

◆ estWorldImuMsgPtr_

nav_msgs::OdometryPtr holistic_fusion::HolisticFusionRos::estWorldImuMsgPtr_
private

Definition at line 172 of file HolisticFusionRos.h.

◆ estWorldImuPathPtr_

nav_msgs::PathPtr holistic_fusion::HolisticFusionRos::estWorldImuPathPtr_
private

Definition at line 180 of file HolisticFusionRos.h.

◆ estWorldPosVarianceMsgPtr_

geometry_msgs::Vector3StampedPtr holistic_fusion::HolisticFusionRos::estWorldPosVarianceMsgPtr_
private

Definition at line 175 of file HolisticFusionRos.h.

◆ estWorldRotVarianceMsgPtr_

geometry_msgs::Vector3StampedPtr holistic_fusion::HolisticFusionRos::estWorldRotVarianceMsgPtr_
private

Definition at line 176 of file HolisticFusionRos.h.

◆ gyroBiasMsgPtr_

geometry_msgs::Vector3StampedPtr holistic_fusion::HolisticFusionRos::gyroBiasMsgPtr_
private

Definition at line 186 of file HolisticFusionRos.h.

◆ landmarkMarkersMsgPtr_

visualization_msgs::MarkerArrayPtr holistic_fusion::HolisticFusionRos::landmarkMarkersMsgPtr_
private

Definition at line 188 of file HolisticFusionRos.h.

◆ lastOptimizedStateTimestamp_

double holistic_fusion::HolisticFusionRos::lastOptimizedStateTimestamp_ = 0.0
private

Definition at line 197 of file HolisticFusionRos.h.

◆ measWorldLidarPathPtr_

nav_msgs::PathPtr holistic_fusion::HolisticFusionRos::measWorldLidarPathPtr_
private

Definition at line 183 of file HolisticFusionRos.h.

◆ optimizationResultLoggingPath

std::string holistic_fusion::HolisticFusionRos::optimizationResultLoggingPath = ""
protected

Definition at line 134 of file HolisticFusionRos.h.

◆ optWorldImuMsgPtr_

nav_msgs::OdometryPtr holistic_fusion::HolisticFusionRos::optWorldImuMsgPtr_
private

Definition at line 173 of file HolisticFusionRos.h.

◆ optWorldImuPathPtr_

nav_msgs::PathPtr holistic_fusion::HolisticFusionRos::optWorldImuPathPtr_
private

Definition at line 181 of file HolisticFusionRos.h.

◆ privateNode

ros::NodeHandle holistic_fusion::HolisticFusionRos::privateNode
protected

Definition at line 121 of file HolisticFusionRos.h.

◆ pubAccelBias_

ros::Publisher holistic_fusion::HolisticFusionRos::pubAccelBias_
private

Definition at line 155 of file HolisticFusionRos.h.

◆ pubAddedImuMeas_

ros::Publisher holistic_fusion::HolisticFusionRos::pubAddedImuMeas_
private

Definition at line 158 of file HolisticFusionRos.h.

◆ pubEstOdomImu_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstOdomImu_
private

Definition at line 139 of file HolisticFusionRos.h.

◆ pubEstOdomImuPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstOdomImuPath_
private

Definition at line 148 of file HolisticFusionRos.h.

◆ pubEstWorldImu_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstWorldImu_
private

Definition at line 140 of file HolisticFusionRos.h.

◆ pubEstWorldImuPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstWorldImuPath_
private

Definition at line 149 of file HolisticFusionRos.h.

◆ pubEstWorldPosVariance_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstWorldPosVariance_
private

Definition at line 143 of file HolisticFusionRos.h.

◆ pubEstWorldRotVariance_

ros::Publisher holistic_fusion::HolisticFusionRos::pubEstWorldRotVariance_
private

Definition at line 144 of file HolisticFusionRos.h.

◆ pubGyroBias_

ros::Publisher holistic_fusion::HolisticFusionRos::pubGyroBias_
private

Definition at line 156 of file HolisticFusionRos.h.

◆ pubLandmarks_

ros::Publisher holistic_fusion::HolisticFusionRos::pubLandmarks_
private

Definition at line 164 of file HolisticFusionRos.h.

◆ pubMeasWorldGnssLPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubMeasWorldGnssLPath_
private

Definition at line 151 of file HolisticFusionRos.h.

◆ pubMeasWorldGnssRPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubMeasWorldGnssRPath_
private

Definition at line 152 of file HolisticFusionRos.h.

◆ pubMeasWorldLidarPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubMeasWorldLidarPath_
private

Definition at line 153 of file HolisticFusionRos.h.

◆ pubOptWorldImu_

ros::Publisher holistic_fusion::HolisticFusionRos::pubOptWorldImu_
private

Definition at line 141 of file HolisticFusionRos.h.

◆ pubOptWorldImuPath_

ros::Publisher holistic_fusion::HolisticFusionRos::pubOptWorldImuPath_
private

Definition at line 150 of file HolisticFusionRos.h.

◆ pubPoseStampedByTopicMap_

std::map<std::string, ros::Publisher> holistic_fusion::HolisticFusionRos::pubPoseStampedByTopicMap_ = {}
private

Definition at line 161 of file HolisticFusionRos.h.

◆ pubVelocityMarker_

ros::Publisher holistic_fusion::HolisticFusionRos::pubVelocityMarker_
private

Definition at line 146 of file HolisticFusionRos.h.

◆ referenceFrameAlignedNameId

std::string holistic_fusion::HolisticFusionRos::referenceFrameAlignedNameId = "_holistic_fusion_aligned"
protected

Definition at line 132 of file HolisticFusionRos.h.

◆ rosPublisherMutex_

std::mutex holistic_fusion::HolisticFusionRos::rosPublisherMutex_
private

Definition at line 200 of file HolisticFusionRos.h.

◆ sensorFrameCorrectedNameId

std::string holistic_fusion::HolisticFusionRos::sensorFrameCorrectedNameId = "_holistic_fusion_corrected"
protected

Definition at line 133 of file HolisticFusionRos.h.

◆ srvLogRealTimeNavStates_

ros::ServiceServer holistic_fusion::HolisticFusionRos::srvLogRealTimeNavStates_
private

Definition at line 194 of file HolisticFusionRos.h.

◆ srvSmootherOptimize_

ros::ServiceServer holistic_fusion::HolisticFusionRos::srvSmootherOptimize_
private

Definition at line 192 of file HolisticFusionRos.h.

◆ startTime

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::HolisticFusionRos::startTime
protected

Definition at line 124 of file HolisticFusionRos.h.

◆ subImu_

ros::Subscriber holistic_fusion::HolisticFusionRos::subImu_
private

Definition at line 167 of file HolisticFusionRos.h.

◆ tfBroadcaster

tf::TransformBroadcaster holistic_fusion::HolisticFusionRos::tfBroadcaster
protected

Definition at line 129 of file HolisticFusionRos.h.

The documentation for this class was generated from the following files: