#include <AnymalEstimator.h>

Inheritance diagram for anymal_se::AnymalEstimator:

[legend]

Collaboration diagram for anymal_se::AnymalEstimator:

[legend]

Public Member Functions
	AnymalEstimator (const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
	~AnymalEstimator ()=default
void	setup ()
Public Member Functions inherited from holistic_fusion::HolisticFusionRos
	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
void	initializePublishers (ros::NodeHandle &privateNode) override
void	initializeSubscribers (ros::NodeHandle &privateNode) override
void	initializeMessages (ros::NodeHandle &privateNode) override
void	initializeServices_ (ros::NodeHandle &privateNode)
void	readParams (const ros::NodeHandle &privateNode) override
bool	srvOfflineSmootherOptimizeCallback (holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res) override
Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
virtual void	initializeServices (ros::NodeHandle &privateNode)
virtual void	imuCallback (const sensor_msgs::Imu::ConstPtr &imuPtr)
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)

Private Member Functions
void	lidarUnaryCallback_ (const nav_msgs::Odometry::ConstPtr &lidar_odom_ptr)
void	lidarBetweenCallback_ (const nav_msgs::Odometry::ConstPtr &lidar_odom_ptr)
void	gnssUnaryCallback_ (const sensor_msgs::NavSatFix::ConstPtr &gnssPtr)
void	leggedBetweenCallback_ (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &leggedOdometryPoseKPtr)
void	leggedVelocityUnaryCallback_ (const nav_msgs::Odometry::ConstPtr &leggedOdometryKPtr)
void	leggedKinematicsCallback_ (const holistic_fusion_anymal_msgs::AnymalState::ConstPtr &anymalStatePtr)

Private Attributes
std::shared_ptr< holistic_fusion::GnssHandler >	gnssHandlerPtr_
std::shared_ptr< holistic_fusion::TrajectoryAlignmentHandler >	trajectoryAlignmentHandler_
std::chrono::time_point< std::chrono::high_resolution_clock >	startTime_
std::chrono::time_point< std::chrono::high_resolution_clock >	currentTime_
double	lioOdometryRate_ = 5.0
double	leggedOdometryBetweenRate_ = 400.0
int	leggedOdometryPoseDownsampleFactor_ = 40
double	leggedOdometryVelocityRate_ = 20.0
int	leggedOdometryVelocityDownsampleFactor_ = 4
double	leggedKinematicsRate_ = 400.0
int	leggedKinematicsDownsampleFactor_ = 10
double	gnssRate_ = 10.0
bool	useGnssUnaryFlag_ = false
bool	useLioUnaryFlag_ = true
bool	useLioBetweenFlag_ = false
bool	useLeggedBetweenFlag_ = false
bool	useLeggedVelocityUnaryFlag_ = false
bool	useLeggedKinematicsFlag_ = false
Eigen::Matrix< double, 6, 1 >	initialSe3AlignmentNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 >	lioSe3AlignmentRandomWalk_ = 0.0 * Eigen::Matrix<double, 6, 1>::Ones()
double	gnssPositionOutlierThreshold_ = 1.0
Eigen::Matrix< double, 6, 1 >	lioPoseUnaryNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 >	lioPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 >	legPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 3, 1 >	legVelocityUnaryNoise_ = 1.0 * Eigen::Matrix<double, 3, 1>::Ones()
Eigen::Matrix< double, 3, 1 >	legKinematicsFootPositionUnaryNoise_ = 1.0 * Eigen::Matrix<double, 3, 1>::Ones()
std::array< long, legNames_.size()>	legInContactForNSteps_ = {0, 0, 0, 0}
std::array< long, legNames_.size()>	legTotalContactsCounter_ = {0, 0, 0, 0}
Eigen::Vector3d	accumulatedGnssCoordinates_ {0.0, 0.0, 0.0}
int	gnssCallbackCounter_ {-1}
std::vector< Eigen::Vector3d >	optionalGnssEnuCoordinatesTrajectory_
std::vector< double >	optionalGnssEnuCoordinatesTimeStamps_
int	lidarUnaryCallbackCounter_ {-1}
int	lidarBetweenCallbackCounter_ {-1}
double	lidarBetweenTimeKm1_ {0.0}
Eigen::Isometry3d	lio_T_M_Lkm1_ = Eigen::Isometry3d::Identity()
int	leggedOdometryPoseCallbackCounter_ {-1}
int	leggedOdometryOdomCallbackCounter_ {-1}
int	leggedKinematicsCallbackCounter_ {-1}
Eigen::Isometry3d	T_O_Bl_km1_ = Eigen::Isometry3d::Identity()
double	legOdometryTimeKm1_ {0.0}
ros::Subscriber	subGnssUnary_
ros::Subscriber	subLioUnary_
ros::Subscriber	subLioBetween_
ros::Subscriber	subLeggedBetween_
ros::Subscriber	subLeggedVelocityUnary_
ros::Subscriber	subLeggedKinematics_
tf::TransformListener	tfListener_
ros::Publisher	pubMeasMapLioPath_
ros::Publisher	pubMeasWorldGnssPath_
ros::Publisher	pubFootContactMarkers_
nav_msgs::PathPtr	measLio_mapLidarPathPtr_
nav_msgs::PathPtr	measGnss_worldGnssPathPtr_
ros::ServiceServer	serverTransformGnssToEnu_

Static Private Attributes
static constexpr std::array< const char *, 4 >	legNames_ = {"LF", "RF", "LH", "RH"}
static constexpr long	legInContactDebounceThreshold_ = 3
static constexpr bool	logOptionalGnssEnuCoordinatesFlag_ = false

Additional Inherited Members
Static Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
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 inherited from holistic_fusion::HolisticFusionRos
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_

Detailed Description

Definition at line 42 of file AnymalEstimator.h.

Constructor & Destructor Documentation

◆ AnymalEstimator()

anymal_se::AnymalEstimator::AnymalEstimator ( const std::shared_ptr< ros::NodeHandle > & privateNodePtr )

Definition at line 28 of file AnymalEstimator.cpp.

◆ ~AnymalEstimator()

anymal_se::AnymalEstimator::~AnymalEstimator ( )

default

Member Function Documentation

◆ gnssUnaryCallback_()

void anymal_se::AnymalEstimator::gnssUnaryCallback_ ( const sensor_msgs::NavSatFix::ConstPtr & gnssPtr )

private

Add GNSS to Path

Publish path

Definition at line 190 of file AnymalEstimator.cpp.

◆ initializeMessages()

void anymal_se::AnymalEstimator::initializeMessages ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 120 of file AnymalEstimator.cpp.

◆ initializePublishers()

void anymal_se::AnymalEstimator::initializePublishers ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 64 of file AnymalEstimator.cpp.

◆ initializeServices_()

void anymal_se::AnymalEstimator::initializeServices_ ( ros::NodeHandle & privateNode )

protected

Definition at line 126 of file AnymalEstimator.cpp.

◆ initializeSubscribers()

void anymal_se::AnymalEstimator::initializeSubscribers ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 73 of file AnymalEstimator.cpp.

◆ leggedBetweenCallback_()

void anymal_se::AnymalEstimator::leggedBetweenCallback_ ( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & leggedOdometryPoseKPtr )

private

Definition at line 400 of file AnymalEstimator.cpp.

◆ leggedKinematicsCallback_()

void anymal_se::AnymalEstimator::leggedKinematicsCallback_ ( const holistic_fusion_anymal_msgs::AnymalState::ConstPtr & anymalStatePtr )

private

Definition at line 505 of file AnymalEstimator.cpp.

◆ leggedVelocityUnaryCallback_()

void anymal_se::AnymalEstimator::leggedVelocityUnaryCallback_ ( const nav_msgs::Odometry::ConstPtr & leggedOdometryKPtr )

private

Definition at line 458 of file AnymalEstimator.cpp.

◆ lidarBetweenCallback_()

void anymal_se::AnymalEstimator::lidarBetweenCallback_ ( const nav_msgs::Odometry::ConstPtr & lidar_odom_ptr )

private

Definition at line 335 of file AnymalEstimator.cpp.

◆ lidarUnaryCallback_()

void anymal_se::AnymalEstimator::lidarUnaryCallback_ ( const nav_msgs::Odometry::ConstPtr & lidar_odom_ptr )

private

Definition at line 294 of file AnymalEstimator.cpp.

◆ readParams()

void anymal_se::AnymalEstimator::readParams ( const ros::NodeHandle & privateNode )

overrideprotectedvirtual

LiDAR Odometry

LiDAR Odometry as Between

Legged Odometry

Legged Velocity Unary

Legged Kinematics Foot Position Unary

LiDAR frame

Legged Odometry frame

Gnss frame

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 20 of file readParams.cpp.

◆ setup()

void anymal_se::AnymalEstimator::setup ( )

Definition at line 39 of file AnymalEstimator.cpp.

◆ srvOfflineSmootherOptimizeCallback()

bool anymal_se::AnymalEstimator::srvOfflineSmootherOptimizeCallback	(	holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &	req,
		holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &	res )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 132 of file AnymalEstimator.cpp.

Member Data Documentation

◆ accumulatedGnssCoordinates_

Eigen::Vector3d anymal_se::AnymalEstimator::accumulatedGnssCoordinates_ {0.0, 0.0, 0.0}

private

Definition at line 127 of file AnymalEstimator.h.

◆ currentTime_

std::chrono::time_point<std::chrono::high_resolution_clock> anymal_se::AnymalEstimator::currentTime_

private

Definition at line 85 of file AnymalEstimator.h.

◆ gnssCallbackCounter_

int anymal_se::AnymalEstimator::gnssCallbackCounter_ {-1}

private

Definition at line 128 of file AnymalEstimator.h.

◆ gnssHandlerPtr_

std::shared_ptr<holistic_fusion::GnssHandler> anymal_se::AnymalEstimator::gnssHandlerPtr_

private

Definition at line 78 of file AnymalEstimator.h.

◆ gnssPositionOutlierThreshold_

double anymal_se::AnymalEstimator::gnssPositionOutlierThreshold_ = 1.0

private

Definition at line 112 of file AnymalEstimator.h.

◆ gnssRate_

double anymal_se::AnymalEstimator::gnssRate_ = 10.0

private

Definition at line 97 of file AnymalEstimator.h.

◆ initialSe3AlignmentNoise_

Eigen::Matrix<double, 6, 1> anymal_se::AnymalEstimator::initialSe3AlignmentNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()

private

Definition at line 108 of file AnymalEstimator.h.

◆ leggedKinematicsCallbackCounter_

int anymal_se::AnymalEstimator::leggedKinematicsCallbackCounter_ {-1}

private

Definition at line 142 of file AnymalEstimator.h.

◆ leggedKinematicsDownsampleFactor_

int anymal_se::AnymalEstimator::leggedKinematicsDownsampleFactor_ = 10

private

Definition at line 96 of file AnymalEstimator.h.

◆ leggedKinematicsRate_

double anymal_se::AnymalEstimator::leggedKinematicsRate_ = 400.0

private

Definition at line 95 of file AnymalEstimator.h.

◆ leggedOdometryBetweenRate_

double anymal_se::AnymalEstimator::leggedOdometryBetweenRate_ = 400.0

private

Definition at line 91 of file AnymalEstimator.h.

◆ leggedOdometryOdomCallbackCounter_

int anymal_se::AnymalEstimator::leggedOdometryOdomCallbackCounter_ {-1}

private

Definition at line 141 of file AnymalEstimator.h.

◆ leggedOdometryPoseCallbackCounter_

int anymal_se::AnymalEstimator::leggedOdometryPoseCallbackCounter_ {-1}

private

Definition at line 140 of file AnymalEstimator.h.

◆ leggedOdometryPoseDownsampleFactor_

int anymal_se::AnymalEstimator::leggedOdometryPoseDownsampleFactor_ = 40

private

Definition at line 92 of file AnymalEstimator.h.

◆ leggedOdometryVelocityDownsampleFactor_

int anymal_se::AnymalEstimator::leggedOdometryVelocityDownsampleFactor_ = 4

private

Definition at line 94 of file AnymalEstimator.h.

◆ leggedOdometryVelocityRate_

double anymal_se::AnymalEstimator::leggedOdometryVelocityRate_ = 20.0

private

Definition at line 93 of file AnymalEstimator.h.

◆ legInContactDebounceThreshold_

long anymal_se::AnymalEstimator::legInContactDebounceThreshold_ = 3

staticconstexprprivate

Definition at line 123 of file AnymalEstimator.h.

◆ legInContactForNSteps_

std::array<long, legNames_.size()> anymal_se::AnymalEstimator::legInContactForNSteps_ = {0, 0, 0, 0}

private

Definition at line 121 of file AnymalEstimator.h.

◆ legKinematicsFootPositionUnaryNoise_

Eigen::Matrix<double, 3, 1> anymal_se::AnymalEstimator::legKinematicsFootPositionUnaryNoise_ = 1.0 * Eigen::Matrix<double, 3, 1>::Ones()

private

Definition at line 117 of file AnymalEstimator.h.

◆ legNames_

std::array< const char *, 4 > anymal_se::AnymalEstimator::legNames_ = {"LF", "RF", "LH", "RH"}

staticconstexprprivate

Definition at line 120 of file AnymalEstimator.h.

◆ legOdometryTimeKm1_

double anymal_se::AnymalEstimator::legOdometryTimeKm1_ {0.0}

private

Definition at line 144 of file AnymalEstimator.h.

◆ legPoseBetweenNoise_

Eigen::Matrix<double, 6, 1> anymal_se::AnymalEstimator::legPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()

private

Definition at line 115 of file AnymalEstimator.h.

◆ legTotalContactsCounter_

std::array<long, legNames_.size()> anymal_se::AnymalEstimator::legTotalContactsCounter_ = {0, 0, 0, 0}

private

Definition at line 122 of file AnymalEstimator.h.

◆ legVelocityUnaryNoise_

Eigen::Matrix<double, 3, 1> anymal_se::AnymalEstimator::legVelocityUnaryNoise_ = 1.0 * Eigen::Matrix<double, 3, 1>::Ones()

private

Definition at line 116 of file AnymalEstimator.h.

◆ lidarBetweenCallbackCounter_

int anymal_se::AnymalEstimator::lidarBetweenCallbackCounter_ {-1}

private

Definition at line 136 of file AnymalEstimator.h.

◆ lidarBetweenTimeKm1_

double anymal_se::AnymalEstimator::lidarBetweenTimeKm1_ {0.0}

private

Definition at line 137 of file AnymalEstimator.h.

◆ lidarUnaryCallbackCounter_

int anymal_se::AnymalEstimator::lidarUnaryCallbackCounter_ {-1}

private

Definition at line 134 of file AnymalEstimator.h.

◆ lio_T_M_Lkm1_

Eigen::Isometry3d anymal_se::AnymalEstimator::lio_T_M_Lkm1_ = Eigen::Isometry3d::Identity()

private

Definition at line 138 of file AnymalEstimator.h.

◆ lioOdometryRate_

double anymal_se::AnymalEstimator::lioOdometryRate_ = 5.0

private

Definition at line 90 of file AnymalEstimator.h.

◆ lioPoseBetweenNoise_

Eigen::Matrix<double, 6, 1> anymal_se::AnymalEstimator::lioPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()

private

Definition at line 114 of file AnymalEstimator.h.

◆ lioPoseUnaryNoise_

Eigen::Matrix<double, 6, 1> anymal_se::AnymalEstimator::lioPoseUnaryNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()

private

Definition at line 113 of file AnymalEstimator.h.

◆ lioSe3AlignmentRandomWalk_

Eigen::Matrix<double, 6, 1> anymal_se::AnymalEstimator::lioSe3AlignmentRandomWalk_ = 0.0 * Eigen::Matrix<double, 6, 1>::Ones()

private

Definition at line 109 of file AnymalEstimator.h.

◆ logOptionalGnssEnuCoordinatesFlag_

bool anymal_se::AnymalEstimator::logOptionalGnssEnuCoordinatesFlag_ = false

staticconstexprprivate

Definition at line 131 of file AnymalEstimator.h.

◆ measGnss_worldGnssPathPtr_

nav_msgs::PathPtr anymal_se::AnymalEstimator::measGnss_worldGnssPathPtr_

private

Definition at line 167 of file AnymalEstimator.h.

◆ measLio_mapLidarPathPtr_

nav_msgs::PathPtr anymal_se::AnymalEstimator::measLio_mapLidarPathPtr_

private

Definition at line 166 of file AnymalEstimator.h.

◆ optionalGnssEnuCoordinatesTimeStamps_

std::vector<double> anymal_se::AnymalEstimator::optionalGnssEnuCoordinatesTimeStamps_

private

Definition at line 130 of file AnymalEstimator.h.

◆ optionalGnssEnuCoordinatesTrajectory_

std::vector<Eigen::Vector3d> anymal_se::AnymalEstimator::optionalGnssEnuCoordinatesTrajectory_

private

Definition at line 129 of file AnymalEstimator.h.

◆ pubFootContactMarkers_

ros::Publisher anymal_se::AnymalEstimator::pubFootContactMarkers_

private

Definition at line 163 of file AnymalEstimator.h.

◆ pubMeasMapLioPath_

ros::Publisher anymal_se::AnymalEstimator::pubMeasMapLioPath_

private

Definition at line 160 of file AnymalEstimator.h.

◆ pubMeasWorldGnssPath_

ros::Publisher anymal_se::AnymalEstimator::pubMeasWorldGnssPath_

private

Definition at line 161 of file AnymalEstimator.h.

◆ serverTransformGnssToEnu_

ros::ServiceServer anymal_se::AnymalEstimator::serverTransformGnssToEnu_

private

Definition at line 170 of file AnymalEstimator.h.

◆ startTime_

std::chrono::time_point<std::chrono::high_resolution_clock> anymal_se::AnymalEstimator::startTime_

private

Definition at line 84 of file AnymalEstimator.h.

◆ subGnssUnary_

ros::Subscriber anymal_se::AnymalEstimator::subGnssUnary_

private

Definition at line 150 of file AnymalEstimator.h.

◆ subLeggedBetween_

ros::Subscriber anymal_se::AnymalEstimator::subLeggedBetween_

private

Definition at line 153 of file AnymalEstimator.h.

◆ subLeggedKinematics_

ros::Subscriber anymal_se::AnymalEstimator::subLeggedKinematics_

private

Definition at line 155 of file AnymalEstimator.h.

◆ subLeggedVelocityUnary_

ros::Subscriber anymal_se::AnymalEstimator::subLeggedVelocityUnary_

private

Definition at line 154 of file AnymalEstimator.h.

◆ subLioBetween_

ros::Subscriber anymal_se::AnymalEstimator::subLioBetween_

private

Definition at line 152 of file AnymalEstimator.h.

◆ subLioUnary_

ros::Subscriber anymal_se::AnymalEstimator::subLioUnary_

private

Definition at line 151 of file AnymalEstimator.h.

◆ T_O_Bl_km1_

Eigen::Isometry3d anymal_se::AnymalEstimator::T_O_Bl_km1_ = Eigen::Isometry3d::Identity()

private

Definition at line 143 of file AnymalEstimator.h.

◆ tfListener_

tf::TransformListener anymal_se::AnymalEstimator::tfListener_

private

Definition at line 156 of file AnymalEstimator.h.

◆ trajectoryAlignmentHandler_

std::shared_ptr<holistic_fusion::TrajectoryAlignmentHandler> anymal_se::AnymalEstimator::trajectoryAlignmentHandler_

private

Definition at line 81 of file AnymalEstimator.h.

◆ useGnssUnaryFlag_

bool anymal_se::AnymalEstimator::useGnssUnaryFlag_ = false

private

Definition at line 100 of file AnymalEstimator.h.

◆ useLeggedBetweenFlag_

bool anymal_se::AnymalEstimator::useLeggedBetweenFlag_ = false

private

Definition at line 103 of file AnymalEstimator.h.

◆ useLeggedKinematicsFlag_

bool anymal_se::AnymalEstimator::useLeggedKinematicsFlag_ = false

private

Definition at line 105 of file AnymalEstimator.h.

◆ useLeggedVelocityUnaryFlag_

bool anymal_se::AnymalEstimator::useLeggedVelocityUnaryFlag_ = false

private

Definition at line 104 of file AnymalEstimator.h.

◆ useLioBetweenFlag_

bool anymal_se::AnymalEstimator::useLioBetweenFlag_ = false

private

Definition at line 102 of file AnymalEstimator.h.

◆ useLioUnaryFlag_

bool anymal_se::AnymalEstimator::useLioUnaryFlag_ = true

private

Definition at line 101 of file AnymalEstimator.h.

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

ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h
ros/examples/anymal_estimator_graph/src/lib/AnymalEstimator.cpp
ros/examples/anymal_estimator_graph/src/lib/readParams.cpp

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ AnymalEstimator()

◆ ~AnymalEstimator()

Member Function Documentation

◆ gnssUnaryCallback_()

◆ initializeMessages()

◆ initializePublishers()

◆ initializeServices_()

◆ initializeSubscribers()

◆ leggedBetweenCallback_()

◆ leggedKinematicsCallback_()

◆ leggedVelocityUnaryCallback_()

◆ lidarBetweenCallback_()

◆ lidarUnaryCallback_()

◆ readParams()

◆ setup()

◆ srvOfflineSmootherOptimizeCallback()

Member Data Documentation

◆ accumulatedGnssCoordinates_

◆ currentTime_

◆ gnssCallbackCounter_

◆ gnssHandlerPtr_

◆ gnssPositionOutlierThreshold_

◆ gnssRate_

◆ initialSe3AlignmentNoise_

◆ leggedKinematicsCallbackCounter_

◆ leggedKinematicsDownsampleFactor_

◆ leggedKinematicsRate_

◆ leggedOdometryBetweenRate_

◆ leggedOdometryOdomCallbackCounter_

◆ leggedOdometryPoseCallbackCounter_

◆ leggedOdometryPoseDownsampleFactor_

◆ leggedOdometryVelocityDownsampleFactor_

◆ leggedOdometryVelocityRate_

◆ legInContactDebounceThreshold_

◆ legInContactForNSteps_

◆ legKinematicsFootPositionUnaryNoise_

◆ legNames_

◆ legOdometryTimeKm1_

◆ legPoseBetweenNoise_

◆ legTotalContactsCounter_

◆ legVelocityUnaryNoise_

◆ lidarBetweenCallbackCounter_

◆ lidarBetweenTimeKm1_

◆ lidarUnaryCallbackCounter_

◆ lio_T_M_Lkm1_

◆ lioOdometryRate_

◆ lioPoseBetweenNoise_

◆ lioPoseUnaryNoise_

◆ lioSe3AlignmentRandomWalk_

◆ logOptionalGnssEnuCoordinatesFlag_

◆ measGnss_worldGnssPathPtr_

◆ measLio_mapLidarPathPtr_

◆ optionalGnssEnuCoordinatesTimeStamps_

◆ optionalGnssEnuCoordinatesTrajectory_

◆ pubFootContactMarkers_

◆ pubMeasMapLioPath_

◆ pubMeasWorldGnssPath_

◆ serverTransformGnssToEnu_

◆ startTime_

◆ subGnssUnary_

◆ subLeggedBetween_

◆ subLeggedKinematics_

◆ subLeggedVelocityUnary_

◆ subLioBetween_

◆ subLioUnary_

◆ T_O_Bl_km1_

◆ tfListener_

◆ trajectoryAlignmentHandler_

◆ useGnssUnaryFlag_

◆ useLeggedBetweenFlag_

◆ useLeggedKinematicsFlag_

◆ useLeggedVelocityUnaryFlag_