HolisticFusionRos.h

Go to the documentation of this file.

/*
Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
All rights reserved.
This file is released under the "BSD-3-Clause License".
Please see the LICENSE file that has been included as part of this package.
 */
 
#ifndef HOLISTICFUSIONROS_H
#define HOLISTICFUSIONROS_H
 
// std
#include <chrono>
 
// ROS
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_srvs/Trigger.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
 
// Workspace
#include "holistic_fusion/interface/HolisticFusion.h"
#include "holistic_fusion/interface/HolisticFusionClassic.h"
#include "holistic_fusion/interface/HolisticFusionHolistic.h"
#include "holistic_fusion_ros_msgs/OfflineOptimizationTrigger.h"
 
// Macros
#define ROS_QUEUE_SIZE 1
#define NUM_KEEP_MARKER_NAMES 10   // Simply used to detect duplications (with 4 legs 10 is enough)
#define NUM_MARKERS_IN_ARRAY 1000  // Actual number of markers in array
 
namespace holistic_fusion {
 
class HolisticFusionRos : public HolisticFusionClassic, public HolisticFusionHolistic {
 public:
  explicit HolisticFusionRos(const std::shared_ptr<ros::NodeHandle>& privateNodePtr);
  // Destructor
  ~HolisticFusionRos() override = default;
 
  // Setup
  void setup(const std::shared_ptr<StaticTransforms> staticTransformsPtr);
 
 protected:
  // Functions that need implementation
  virtual void initializePublishers(ros::NodeHandle& privateNode);
  virtual void initializeSubscribers(ros::NodeHandle& privateNode);
  virtual void initializeMessages(ros::NodeHandle& privateNode);
  virtual void initializeServices(ros::NodeHandle& privateNode);
 
  // Parameter Loading -----------------------------------
  virtual void readParams(const ros::NodeHandle& privateNode);
 
  // Commodity Functions to be shared -----------------------------------
 
  // Static
  // Add to Topics
  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);
 
  // Extract from State
  static void extractCovariancesFromOptimizedState(
      Eigen::Matrix<double, 6, 6>& poseCovarianceRos, Eigen::Matrix<double, 6, 6>& twistCovarianceRos,
      const std::shared_ptr<holistic_fusion::SafeNavStateWithCovarianceAndBias>& optimizedStateWithCovarianceAndBiasPtr);
  // Markers
  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);
 
  // Callbacks
  virtual void imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr);
 
  // Services
  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);
 
  // Publishing -----------------------------------
  // Higher Level Functions
  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);
 
  // Lower Level Functions
  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;
 
  // Measure time
  long secondsSinceStart();
 
  // Node
  ros::NodeHandle privateNode;
 
  // Time
  std::chrono::time_point<std::chrono::high_resolution_clock> startTime;
  std::chrono::time_point<std::chrono::high_resolution_clock> currentTime;
 
  // Publishers
  // TF
  tf::TransformBroadcaster tfBroadcaster;
 
  // Members
  std::string referenceFrameAlignedNameId = "_holistic_fusion_aligned";
  std::string sensorFrameCorrectedNameId = "_holistic_fusion_corrected";
  std::string optimizationResultLoggingPath = "";
 
 private:
  // Publishers
  // Odometry
  ros::Publisher pubEstOdomImu_;
  ros::Publisher pubEstWorldImu_;
  ros::Publisher pubOptWorldImu_;
  // Vector3 Variances
  ros::Publisher pubEstWorldPosVariance_;
  ros::Publisher pubEstWorldRotVariance_;
  // Velocity Markers
  ros::Publisher pubVelocityMarker_;
  // Path
  ros::Publisher pubEstOdomImuPath_;
  ros::Publisher pubEstWorldImuPath_;
  ros::Publisher pubOptWorldImuPath_;
  ros::Publisher pubMeasWorldGnssLPath_;
  ros::Publisher pubMeasWorldGnssRPath_;
  ros::Publisher pubMeasWorldLidarPath_;
  // Imu Bias
  ros::Publisher pubAccelBias_;
  ros::Publisher pubGyroBias_;
  // Added Imu Measurements
  ros::Publisher pubAddedImuMeas_;
 
  // PoseStamped --> Needs to be dynamic as we do not know the number of sensors
  std::map<std::string, ros::Publisher> pubPoseStampedByTopicMap_ = {};
 
  // Landmarks in Marker Array
  ros::Publisher pubLandmarks_;
 
  // Subscribers
  ros::Subscriber subImu_;
 
  // Messages
  // Odometry
  nav_msgs::OdometryPtr estOdomImuMsgPtr_;
  nav_msgs::OdometryPtr estWorldImuMsgPtr_;
  nav_msgs::OdometryPtr optWorldImuMsgPtr_;
  // Vector3 Variances
  geometry_msgs::Vector3StampedPtr estWorldPosVarianceMsgPtr_;
  geometry_msgs::Vector3StampedPtr estWorldRotVarianceMsgPtr_;
  // Path
  // Estimated
  nav_msgs::PathPtr estOdomImuPathPtr_;
  nav_msgs::PathPtr estWorldImuPathPtr_;
  nav_msgs::PathPtr optWorldImuPathPtr_;
  // Measured
  nav_msgs::PathPtr measWorldLidarPathPtr_;
  // Imu Bias
  geometry_msgs::Vector3StampedPtr accelBiasMsgPtr_;
  geometry_msgs::Vector3StampedPtr gyroBiasMsgPtr_;
  // Marker Array
  visualization_msgs::MarkerArrayPtr landmarkMarkersMsgPtr_;
 
  // Services
  // Trigger offline smoother optimization
  ros::ServiceServer srvSmootherOptimize_;
  // Log Real-Time States
  ros::ServiceServer srvLogRealTimeNavStates_;
 
  // Last Optimized State Timestamp
  double lastOptimizedStateTimestamp_ = 0.0;
 
  // Mutex
  std::mutex rosPublisherMutex_;
};
}  // namespace holistic_fusion
 
#endif  // end HOLISTICFUSIONROS_H