ros_filter.h

Go to the documentation of this file.

/*
 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ROBOT_LOCALIZATION_ROS_FILTER_H
#define ROBOT_LOCALIZATION_ROS_FILTER_H

#include "robot_localization/ros_filter_utilities.h"
#include "robot_localization/filter_common.h"
#include "robot_localization/filter_base.h"

#include <robot_localization/SetPose.h>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/message_filter.h>
#include <tf2/LinearMath/Transform.h>
#include <message_filters/subscriber.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <diagnostic_msgs/DiagnosticStatus.h>

#include <XmlRpcException.h>

#include <Eigen/Dense>

#include <fstream>
#include <map>
#include <numeric>
#include <queue>
#include <string>
#include <vector>
#include <deque>

namespace RobotLocalization
{

struct CallbackData
{
  CallbackData(const std::string &topicName,
               const std::vector<int> &updateVector,
               const int updateSum,
               const bool differential,
               const bool relative,
               const double rejectionThreshold) :
    topicName_(topicName),
    updateVector_(updateVector),
    updateSum_(updateSum),
    differential_(differential),
    relative_(relative),
    rejectionThreshold_(rejectionThreshold)
  {
  }

  std::string topicName_;
  std::vector<int> updateVector_;
  int updateSum_;
  bool differential_;
  bool relative_;
  double rejectionThreshold_;
};

typedef std::priority_queue<MeasurementPtr, std::vector<MeasurementPtr>, Measurement> MeasurementQueue;
typedef std::deque<MeasurementPtr> MeasurementHistoryDeque;
typedef std::deque<FilterStatePtr> FilterStateHistoryDeque;

template<class T> class RosFilter
{
  public:
    explicit RosFilter(std::vector<double> args = std::vector<double>());

    ~RosFilter();

    void reset();

    void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
                              const CallbackData &callbackData,
                              const std::string &targetFrame);

    void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);

    void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);

    void enqueueMeasurement(const std::string &topicName,
                            const Eigen::VectorXd &measurement,
                            const Eigen::MatrixXd &measurementCovariance,
                            const std::vector<int> &updateVector,
                            const double mahalanobisThresh,
                            const ros::Time &time);

    void forceTwoD(Eigen::VectorXd &measurement,
                   Eigen::MatrixXd &measurementCovariance,
                   std::vector<int> &updateVector);

    bool getFilteredOdometryMessage(nav_msgs::Odometry &message);

    bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message);

    void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,
      const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,
      const CallbackData &accelCallbackData);

    void integrateMeasurements(const ros::Time &currentTime);

    void loadParams();

    void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
      const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);

    void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
                      const CallbackData &callbackData,
                      const std::string &targetFrame,
                      const bool imuData);

    void run();

    void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);

    bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
                            robot_localization::SetPose::Response&);

    std::string tfFailureReasonString(const tf2_ros::FilterFailureReason reason);

    void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
                       const CallbackData &callbackData,
                       const std::string &targetFrame);

  protected:
    bool revertTo(const double time);

    void saveFilterState(FilterBase &filter);

    void clearExpiredHistory(const double cutoffTime);

    void addDiagnostic(const int errLevel,
                       const std::string &topicAndClass,
                       const std::string &message,
                       const bool staticDiag);

    void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper);

    void copyCovariance(const double *arr,
                        Eigen::MatrixXd &covariance,
                        const std::string &topicName,
                        const std::vector<int> &updateVector,
                        const size_t offset,
                        const size_t dimension);

    void copyCovariance(const Eigen::MatrixXd &covariance,
                        double *arr,
                        const size_t dimension);

    std::vector<int> loadUpdateConfig(const std::string &topicName);

    bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
                             const std::string &topicName,
                             const std::string &targetFrame,
                             std::vector<int> &updateVector,
                             Eigen::VectorXd &measurement,
                             Eigen::MatrixXd &measurementCovariance);

    bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
                     const std::string &topicName,
                     const std::string &targetFrame,
                     const bool differential,
                     const bool relative,
                     const bool imuData,
                     std::vector<int> &updateVector,
                     Eigen::VectorXd &measurement,
                     Eigen::MatrixXd &measurementCovariance);

    bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
                      const std::string &topicName,
                      const std::string &targetFrame,
                      std::vector<int> &updateVector,
                      Eigen::VectorXd &measurement,
                      Eigen::MatrixXd &measurementCovariance);

    std::string baseLinkFrameId_;

    ros::Subscriber controlSub_;

    std::map<std::string, std::string> staticDiagnostics_;

    std::map<std::string, std::string> dynamicDiagnostics_;

    std::ofstream debugStream_;

    int dynamicDiagErrorLevel_;

    diagnostic_updater::Updater diagnosticUpdater_;

    T filter_;

    double frequency_;

    double historyLength_;

    bool resetOnTimeJump_;

    Eigen::VectorXd latestControl_;

    ros::Time latestControlTime_;

    ros::Duration tfTimeout_;

    std::vector<ros::Subscriber> topicSubs_;

    std::map<std::string, tf2::Transform> initialMeasurements_;

    std::map<std::string, ros::Time> lastMessageTimes_;

    ros::Time lastSetPoseTime_;

    std::string mapFrameId_;

    MeasurementQueue measurementQueue_;

    ros::NodeHandle nh_;

    ros::NodeHandle nhLocal_;

    std::string odomFrameId_;

    std::map<std::string, tf2::Transform> previousMeasurements_;

    std::map<std::string, Eigen::MatrixXd> previousMeasurementCovariances_;

    bool printDiagnostics_;

    std::map<std::string, bool> removeGravitationalAcc_;

    double gravitationalAcc_;

    ros::Subscriber setPoseSub_;

    ros::ServiceServer setPoseSrv_;

    bool smoothLaggedData_;

    std::vector<std::string> stateVariableNames_;

    int staticDiagErrorLevel_;

    tf2_ros::Buffer tfBuffer_;

    tf2_ros::TransformListener tfListener_;

    ros::Duration tfTimeOffset_;

    bool twoDMode_;

    bool useControl_;

    geometry_msgs::TransformStamped worldBaseLinkTransMsg_;

    std::string worldFrameId_;

    bool publishTransform_;

    bool publishAcceleration_;

    //
    // front() refers to the filter state with the earliest timestamp.
    // back() refers to the filter state with the latest timestamp.
    FilterStateHistoryDeque filterStateHistory_;

    // when popped from the measurement priority queue.
    // front() refers to the measurement with the earliest timestamp.
    // back() refers to the measurement with the latest timestamp.
    MeasurementHistoryDeque measurementHistory_;
};

}  // namespace RobotLocalization

#endif  // ROBOT_LOCALIZATION_ROS_FILTER_H