git commit -m "first commit for v2"
This commit is contained in:
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* 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_EKF_H
|
||||
#define ROBOT_LOCALIZATION_EKF_H
|
||||
|
||||
#include "robot_localization/filter_base.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <set>
|
||||
#include <queue>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
//! @brief Extended Kalman filter class
|
||||
//!
|
||||
//! Implementation of an extended Kalman filter (EKF). This
|
||||
//! class derives from FilterBase and overrides the predict()
|
||||
//! and correct() methods in keeping with the discrete time
|
||||
//! EKF algorithm.
|
||||
//!
|
||||
class Ekf: public FilterBase
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor for the Ekf class
|
||||
//!
|
||||
//! @param[in] args - Generic argument container (not used here, but
|
||||
//! needed so that the ROS filters can pass arbitrary arguments to
|
||||
//! templated filter types).
|
||||
//!
|
||||
explicit Ekf(std::vector<double> args = std::vector<double>());
|
||||
|
||||
//! @brief Destructor for the Ekf class
|
||||
//!
|
||||
~Ekf();
|
||||
|
||||
//! @brief Carries out the correct step in the predict/update cycle.
|
||||
//!
|
||||
//! @param[in] measurement - The measurement to fuse with our estimate
|
||||
//!
|
||||
void correct(const Measurement &measurement);
|
||||
|
||||
//! @brief Carries out the predict step in the predict/update cycle.
|
||||
//!
|
||||
//! Projects the state and error matrices forward using a model of
|
||||
//! the vehicle's motion.
|
||||
//!
|
||||
//! @param[in] referenceTime - The time at which the prediction is being made
|
||||
//! @param[in] delta - The time step over which to predict.
|
||||
//!
|
||||
void predict(const double referenceTime, const double delta);
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_EKF_H
|
||||
@@ -0,0 +1,534 @@
|
||||
/*
|
||||
* 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_FILTER_BASE_H
|
||||
#define ROBOT_LOCALIZATION_FILTER_BASE_H
|
||||
|
||||
#include "robot_localization/filter_utilities.h"
|
||||
#include "robot_localization/filter_common.h"
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <map>
|
||||
#include <ostream>
|
||||
#include <queue>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
//! @brief Structure used for storing and comparing measurements
|
||||
//! (for priority queues)
|
||||
//!
|
||||
//! Measurement units are assumed to be in meters and radians.
|
||||
//! Times are real-valued and measured in seconds.
|
||||
//!
|
||||
struct Measurement
|
||||
{
|
||||
// The time stamp of the most recent control term (needed for lagged data)
|
||||
double latestControlTime_;
|
||||
|
||||
// The Mahalanobis distance threshold in number of sigmas
|
||||
double mahalanobisThresh_;
|
||||
|
||||
// The real-valued time, in seconds, since some epoch
|
||||
// (presumably the start of execution, but any will do)
|
||||
double time_;
|
||||
|
||||
// The topic name for this measurement. Needed
|
||||
// for capturing previous state values for new
|
||||
// measurements.
|
||||
std::string topicName_;
|
||||
|
||||
// This defines which variables within this measurement
|
||||
// actually get passed into the filter. std::vector<bool>
|
||||
// is generally frowned upon, so we use ints.
|
||||
std::vector<int> updateVector_;
|
||||
|
||||
// The most recent control vector (needed for lagged data)
|
||||
Eigen::VectorXd latestControl_;
|
||||
|
||||
// The measurement and its associated covariance
|
||||
Eigen::VectorXd measurement_;
|
||||
Eigen::MatrixXd covariance_;
|
||||
|
||||
// We want earlier times to have greater priority
|
||||
bool operator()(const boost::shared_ptr<Measurement> &a, const boost::shared_ptr<Measurement> &b)
|
||||
{
|
||||
return (*this)(*(a.get()), *(b.get()));
|
||||
}
|
||||
|
||||
bool operator()(const Measurement &a, const Measurement &b)
|
||||
{
|
||||
return a.time_ > b.time_;
|
||||
}
|
||||
|
||||
Measurement() :
|
||||
latestControlTime_(0.0),
|
||||
mahalanobisThresh_(std::numeric_limits<double>::max()),
|
||||
time_(0.0),
|
||||
topicName_("")
|
||||
{
|
||||
}
|
||||
};
|
||||
typedef boost::shared_ptr<Measurement> MeasurementPtr;
|
||||
|
||||
//! @brief Structure used for storing and comparing filter states
|
||||
//!
|
||||
//! This structure is useful when higher-level classes need to remember filter history.
|
||||
//! Measurement units are assumed to be in meters and radians.
|
||||
//! Times are real-valued and measured in seconds.
|
||||
//!
|
||||
struct FilterState
|
||||
{
|
||||
// The time stamp of the most recent measurement for the filter
|
||||
double lastMeasurementTime_;
|
||||
|
||||
// The time stamp of the most recent control term
|
||||
double latestControlTime_;
|
||||
|
||||
// The most recent control vector
|
||||
Eigen::VectorXd latestControl_;
|
||||
|
||||
// The filter state vector
|
||||
Eigen::VectorXd state_;
|
||||
|
||||
// The filter error covariance matrix
|
||||
Eigen::MatrixXd estimateErrorCovariance_;
|
||||
|
||||
// We want the queue to be sorted from latest to earliest timestamps.
|
||||
bool operator()(const FilterState &a, const FilterState &b)
|
||||
{
|
||||
return a.lastMeasurementTime_ < b.lastMeasurementTime_;
|
||||
}
|
||||
|
||||
FilterState() :
|
||||
lastMeasurementTime_(0.0),
|
||||
latestControlTime_(0.0)
|
||||
{}
|
||||
};
|
||||
typedef boost::shared_ptr<FilterState> FilterStatePtr;
|
||||
|
||||
class FilterBase
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor for the FilterBase class
|
||||
//!
|
||||
FilterBase();
|
||||
|
||||
//! @brief Destructor for the FilterBase class
|
||||
//!
|
||||
virtual ~FilterBase();
|
||||
|
||||
//! @brief Resets filter to its unintialized state
|
||||
//!
|
||||
void reset();
|
||||
|
||||
//! @brief Computes a dynamic process noise covariance matrix using the parameterized state
|
||||
//!
|
||||
//! This allows us to, e.g., not increase the pose covariance values when the vehicle is not moving
|
||||
//!
|
||||
//! @param[in] state - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance
|
||||
//!
|
||||
void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta);
|
||||
|
||||
//! @brief Carries out the correct step in the predict/update cycle. This method
|
||||
//! must be implemented by subclasses.
|
||||
//!
|
||||
//! @param[in] measurement - The measurement to fuse with the state estimate
|
||||
//!
|
||||
virtual void correct(const Measurement &measurement) = 0;
|
||||
|
||||
//! @brief Returns the control vector currently being used
|
||||
//!
|
||||
//! @return The control vector
|
||||
//!
|
||||
const Eigen::VectorXd& getControl();
|
||||
|
||||
//! @brief Returns the time at which the control term was issued
|
||||
//!
|
||||
//! @return The time the control vector was issued
|
||||
//!
|
||||
double getControlTime();
|
||||
|
||||
//! @brief Gets the value of the debug_ variable.
|
||||
//!
|
||||
//! @return True if in debug mode, false otherwise
|
||||
//!
|
||||
bool getDebug();
|
||||
|
||||
//! @brief Gets the estimate error covariance
|
||||
//!
|
||||
//! @return A copy of the estimate error covariance matrix
|
||||
//!
|
||||
const Eigen::MatrixXd& getEstimateErrorCovariance();
|
||||
|
||||
//! @brief Gets the filter's initialized status
|
||||
//!
|
||||
//! @return True if we've received our first measurement, false otherwise
|
||||
//!
|
||||
bool getInitializedStatus();
|
||||
|
||||
//! @brief Gets the most recent measurement time
|
||||
//!
|
||||
//! @return The time at which we last received a measurement
|
||||
//!
|
||||
double getLastMeasurementTime();
|
||||
|
||||
//! @brief Gets the filter's predicted state, i.e., the
|
||||
//! state estimate before correct() is called.
|
||||
//!
|
||||
//! @return A constant reference to the predicted state
|
||||
//!
|
||||
const Eigen::VectorXd& getPredictedState();
|
||||
|
||||
//! @brief Gets the filter's process noise covariance
|
||||
//!
|
||||
//! @return A constant reference to the process noise covariance
|
||||
//!
|
||||
const Eigen::MatrixXd& getProcessNoiseCovariance();
|
||||
|
||||
//! @brief Gets the sensor timeout value (in seconds)
|
||||
//!
|
||||
//! @return The sensor timeout value
|
||||
//!
|
||||
double getSensorTimeout();
|
||||
|
||||
//! @brief Gets the filter state
|
||||
//!
|
||||
//! @return A constant reference to the current state
|
||||
//!
|
||||
const Eigen::VectorXd& getState();
|
||||
|
||||
//! @brief Carries out the predict step in the predict/update cycle.
|
||||
//! Projects the state and error matrices forward using a model of
|
||||
//! the vehicle's motion. This method must be implemented by subclasses.
|
||||
//!
|
||||
//! @param[in] referenceTime - The time at which the prediction is being made
|
||||
//! @param[in] delta - The time step over which to predict.
|
||||
//!
|
||||
virtual void predict(const double referenceTime, const double delta) = 0;
|
||||
|
||||
//! @brief Does some final preprocessing, carries out the predict/update cycle
|
||||
//!
|
||||
//! @param[in] measurement - The measurement object to fuse into the filter
|
||||
//!
|
||||
virtual void processMeasurement(const Measurement &measurement);
|
||||
|
||||
//! @brief Sets the most recent control term
|
||||
//!
|
||||
//! @param[in] control - The control term to be applied
|
||||
//! @param[in] controlTime - The time at which the control in question was received
|
||||
//!
|
||||
void setControl(const Eigen::VectorXd &control, const double controlTime);
|
||||
|
||||
//! @brief Sets the control update vector and acceleration limits
|
||||
//!
|
||||
//! @param[in] updateVector - The values the control term affects
|
||||
//! @param[in] controlTimeout - Timeout value, in seconds, after which a control is considered stale
|
||||
//! @param[in] accelerationLimits - The acceleration limits for the control variables
|
||||
//! @param[in] accelerationGains - Gains applied to the control term-derived acceleration
|
||||
//! @param[in] decelerationLimits - The deceleration limits for the control variables
|
||||
//! @param[in] decelerationGains - Gains applied to the control term-derived deceleration
|
||||
//!
|
||||
void setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
|
||||
const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
|
||||
const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains);
|
||||
|
||||
//! @brief Sets the filter into debug mode
|
||||
//!
|
||||
//! NOTE: this will generates a lot of debug output to the provided stream.
|
||||
//! The value must be a pointer to a valid ostream object.
|
||||
//!
|
||||
//! @param[in] debug - Whether or not to place the filter in debug mode
|
||||
//! @param[in] outStream - If debug is true, then this must have a valid pointer.
|
||||
//! If the pointer is invalid, the filter will not enter debug mode. If debug is
|
||||
//! false, outStream is ignored.
|
||||
//!
|
||||
void setDebug(const bool debug, std::ostream *outStream = NULL);
|
||||
|
||||
//! @brief Enables dynamic process noise covariance calculation
|
||||
//!
|
||||
//! @param[in] dynamicProcessNoiseCovariance - Whether or not to compute dynamic process noise covariance matrices
|
||||
//!
|
||||
void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance);
|
||||
|
||||
//! @brief Manually sets the filter's estimate error covariance
|
||||
//!
|
||||
//! @param[in] estimateErrorCovariance - The state to set as the filter's current state
|
||||
//!
|
||||
void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance);
|
||||
|
||||
//! @brief Sets the filter's last measurement time.
|
||||
//!
|
||||
//! @param[in] lastMeasurementTime - The last measurement time of the filter
|
||||
//!
|
||||
void setLastMeasurementTime(const double lastMeasurementTime);
|
||||
|
||||
//! @brief Sets the process noise covariance for the filter.
|
||||
//!
|
||||
//! This enables external initialization, which is important, as this
|
||||
//! matrix can be difficult to tune for a given implementation.
|
||||
//!
|
||||
//! @param[in] processNoiseCovariance - The STATE_SIZExSTATE_SIZE process noise covariance matrix
|
||||
//! to use for the filter
|
||||
//!
|
||||
void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance);
|
||||
|
||||
//! @brief Sets the sensor timeout
|
||||
//!
|
||||
//! @param[in] sensorTimeout - The time, in seconds, for a sensor to be
|
||||
//! considered having timed out
|
||||
//!
|
||||
void setSensorTimeout(const double sensorTimeout);
|
||||
|
||||
//! @brief Manually sets the filter's state
|
||||
//!
|
||||
//! @param[in] state - The state to set as the filter's current state
|
||||
//!
|
||||
void setState(const Eigen::VectorXd &state);
|
||||
|
||||
//! @brief Ensures a given time delta is valid (helps with bag file playback issues)
|
||||
//!
|
||||
//! @param[in] delta - The time delta, in seconds, to validate
|
||||
//!
|
||||
void validateDelta(double &delta);
|
||||
|
||||
protected:
|
||||
//! @brief Method for settings bounds on acceleration values derived from controls
|
||||
//! @param[in] state - The current state variable (e.g., linear X velocity)
|
||||
//! @param[in] control - The current control commanded velocity corresponding to the state variable
|
||||
//! @param[in] accelerationLimit - Limit for acceleration (regardless of driving direction)
|
||||
//! @param[in] accelerationGain - Gain applied to acceleration control error
|
||||
//! @param[in] decelerationLimit - Limit for deceleration (moving towards zero, regardless of driving direction)
|
||||
//! @param[in] decelerationGain - Gain applied to deceleration control error
|
||||
//! @return a usable acceleration estimate for the control vector
|
||||
//!
|
||||
inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit,
|
||||
const double accelerationGain, const double decelerationLimit, const double decelerationGain)
|
||||
{
|
||||
FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");
|
||||
|
||||
const double error = control - state;
|
||||
const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01);
|
||||
const double setPoint = (sameSign ? control : 0.0);
|
||||
const bool decelerating = ::fabs(setPoint) < ::fabs(state);
|
||||
double limit = accelerationLimit;
|
||||
double gain = accelerationGain;
|
||||
|
||||
if (decelerating)
|
||||
{
|
||||
limit = decelerationLimit;
|
||||
gain = decelerationGain;
|
||||
}
|
||||
|
||||
const double finalAccel = std::min(std::max(gain * error, -limit), limit);
|
||||
|
||||
FB_DEBUG("Control value: " << control << "\n" <<
|
||||
"State value: " << state << "\n" <<
|
||||
"Error: " << error << "\n" <<
|
||||
"Same sign: " << (sameSign ? "true" : "false") << "\n" <<
|
||||
"Set point: " << setPoint << "\n" <<
|
||||
"Decelerating: " << (decelerating ? "true" : "false") << "\n" <<
|
||||
"Limit: " << limit << "\n" <<
|
||||
"Gain: " << gain << "\n" <<
|
||||
"Final is " << finalAccel << "\n");
|
||||
|
||||
return finalAccel;
|
||||
}
|
||||
|
||||
//! @brief Keeps the state Euler angles in the range [-pi, pi]
|
||||
//!
|
||||
virtual void wrapStateAngles();
|
||||
|
||||
//! @brief Tests if innovation is within N-sigmas of covariance. Returns true if passed the test.
|
||||
//! @param[in] innovation - The difference between the measurement and the state
|
||||
//! @param[in] invCovariance - The innovation error
|
||||
//! @param[in] nsigmas - Number of standard deviations that are considered acceptable
|
||||
//!
|
||||
virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
|
||||
const Eigen::MatrixXd &invCovariance,
|
||||
const double nsigmas);
|
||||
|
||||
//! @brief Converts the control term to an acceleration to be applied in the prediction step
|
||||
//! @param[in] referenceTime - The time of the update (measurement used in the prediction step)
|
||||
//! @param[in] predictionDelta - The amount of time over which we are carrying out our prediction
|
||||
//!
|
||||
void prepareControl(const double referenceTime, const double predictionDelta);
|
||||
|
||||
//! @brief Whether or not we've received any measurements
|
||||
//!
|
||||
bool initialized_;
|
||||
|
||||
//! @brief Whether or not we apply the control term
|
||||
//!
|
||||
bool useControl_;
|
||||
|
||||
//! @brief If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a
|
||||
//! dynamic process noise covariance matrix
|
||||
//!
|
||||
bool useDynamicProcessNoiseCovariance_;
|
||||
|
||||
//! @brief Tracks the time the filter was last updated using a measurement.
|
||||
//!
|
||||
//! This value is used to monitor sensor readings with respect to the sensorTimeout_.
|
||||
//! We also use it to compute the time delta values for our prediction step.
|
||||
//!
|
||||
double lastMeasurementTime_;
|
||||
|
||||
//! @brief The time of reception of the most recent control term
|
||||
//!
|
||||
double latestControlTime_;
|
||||
|
||||
//! @brief Timeout value, in seconds, after which a control is considered stale
|
||||
//!
|
||||
double controlTimeout_;
|
||||
|
||||
//! @brief The updates to the filter - both predict and correct - are driven
|
||||
//! by measurements. If we get a gap in measurements for some reason, we want
|
||||
//! the filter to continue estimating. When this gap occurs, as specified by
|
||||
//! this timeout, we will continue to call predict() at the filter's frequency.
|
||||
//!
|
||||
double sensorTimeout_;
|
||||
|
||||
//! @brief Which control variables are being used (e.g., not every vehicle is controllable in Y or Z)
|
||||
//!
|
||||
std::vector<int> controlUpdateVector_;
|
||||
|
||||
//! @brief Gains applied to acceleration derived from control term
|
||||
//!
|
||||
std::vector<double> accelerationGains_;
|
||||
|
||||
//! @brief Caps the acceleration we apply from control input
|
||||
//!
|
||||
std::vector<double> accelerationLimits_;
|
||||
|
||||
//! @brief Gains applied to deceleration derived from control term
|
||||
//!
|
||||
std::vector<double> decelerationGains_;
|
||||
|
||||
//! @brief Caps the deceleration we apply from control input
|
||||
//!
|
||||
std::vector<double> decelerationLimits_;
|
||||
|
||||
//! @brief Variable that gets updated every time we process a measurement and we have a valid control
|
||||
//!
|
||||
Eigen::VectorXd controlAcceleration_;
|
||||
|
||||
//! @brief Latest control term
|
||||
//!
|
||||
Eigen::VectorXd latestControl_;
|
||||
|
||||
//! @brief Holds the last predicted state of the filter
|
||||
//!
|
||||
Eigen::VectorXd predictedState_;
|
||||
|
||||
//! @brief This is the robot's state vector, which is what we are trying to
|
||||
//! filter. The values in this vector are what get reported by the node.
|
||||
//!
|
||||
Eigen::VectorXd state_;
|
||||
|
||||
//! @brief Covariance matrices can be incredibly unstable. We can
|
||||
//! add a small value to it at each iteration to help maintain its
|
||||
//! positive-definite property.
|
||||
//!
|
||||
Eigen::MatrixXd covarianceEpsilon_;
|
||||
|
||||
//! @brief Gets updated when useDynamicProcessNoise_ is true
|
||||
//!
|
||||
Eigen::MatrixXd dynamicProcessNoiseCovariance_;
|
||||
|
||||
//! @brief This matrix stores the total error in our position
|
||||
//! estimate (the state_ variable).
|
||||
//!
|
||||
Eigen::MatrixXd estimateErrorCovariance_;
|
||||
|
||||
//! @brief We need the identity for a few operations. Better to store it.
|
||||
//!
|
||||
Eigen::MatrixXd identity_;
|
||||
|
||||
//! @brief As we move through the world, we follow a predict/update
|
||||
//! cycle. If one were to imagine a scenario where all we did was make
|
||||
//! predictions without correcting, the error in our position estimate
|
||||
//! would grow without bound. This error is stored in the
|
||||
//! stateEstimateCovariance_ matrix. However, this matrix doesn't answer
|
||||
//! the question of *how much* our error should grow for each time step.
|
||||
//! That's where the processNoiseCovariance matrix comes in. When we
|
||||
//! make a prediction using the transfer function, we add this matrix
|
||||
//! (times deltaT) to the state estimate covariance matrix.
|
||||
//!
|
||||
Eigen::MatrixXd processNoiseCovariance_;
|
||||
|
||||
//! @brief The Kalman filter transfer function
|
||||
//!
|
||||
//! Kalman filters and extended Kalman filters project the current
|
||||
//! state forward in time. This is the "predict" part of the predict/correct
|
||||
//! cycle. A Kalman filter has a (typically constant) matrix A that defines
|
||||
//! how to turn the current state, x, into the predicted next state. For an
|
||||
//! EKF, this matrix becomes a function f(x). However, this function can still
|
||||
//! be expressed as a matrix to make the math a little cleaner, which is what
|
||||
//! we do here. Technically, each row in the matrix is actually a function.
|
||||
//! Some rows will contain many trigonometric functions, which are of course
|
||||
//! non-linear. In any case, you can think of this as the 'A' matrix in the
|
||||
//! Kalman filter formulation.
|
||||
//!
|
||||
Eigen::MatrixXd transferFunction_;
|
||||
|
||||
//! @brief The Kalman filter transfer function Jacobian
|
||||
//!
|
||||
//! The transfer function is allowed to be non-linear in an EKF, but
|
||||
//! for propagating (predicting) the covariance matrix, we need to linearize
|
||||
//! it about the current mean (i.e., state). This is done via a Jacobian,
|
||||
//! which calculates partial derivatives of each row of the transfer function
|
||||
//! matrix with respect to each state variable.
|
||||
//!
|
||||
Eigen::MatrixXd transferFunctionJacobian_;
|
||||
|
||||
//! @brief Used for outputting debug messages
|
||||
//!
|
||||
std::ostream *debugStream_;
|
||||
|
||||
private:
|
||||
//! @brief Whether or not the filter is in debug mode
|
||||
//!
|
||||
bool debug_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_FILTER_BASE_H
|
||||
@@ -0,0 +1,97 @@
|
||||
/*
|
||||
* 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_FILTER_COMMON_H
|
||||
#define ROBOT_LOCALIZATION_FILTER_COMMON_H
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
//! @brief Enumeration that defines the state vector
|
||||
//!
|
||||
enum StateMembers
|
||||
{
|
||||
StateMemberX = 0,
|
||||
StateMemberY,
|
||||
StateMemberZ,
|
||||
StateMemberRoll,
|
||||
StateMemberPitch,
|
||||
StateMemberYaw,
|
||||
StateMemberVx,
|
||||
StateMemberVy,
|
||||
StateMemberVz,
|
||||
StateMemberVroll,
|
||||
StateMemberVpitch,
|
||||
StateMemberVyaw,
|
||||
StateMemberAx,
|
||||
StateMemberAy,
|
||||
StateMemberAz
|
||||
};
|
||||
|
||||
//! @brief Enumeration that defines the control vector
|
||||
//!
|
||||
enum ControlMembers
|
||||
{
|
||||
ControlMemberVx,
|
||||
ControlMemberVy,
|
||||
ControlMemberVz,
|
||||
ControlMemberVroll,
|
||||
ControlMemberVpitch,
|
||||
ControlMemberVyaw
|
||||
};
|
||||
|
||||
//! @brief Global constants that define our state
|
||||
//! vector size and offsets to groups of values
|
||||
//! within that state.
|
||||
const int STATE_SIZE = 15;
|
||||
const int POSITION_OFFSET = StateMemberX;
|
||||
const int ORIENTATION_OFFSET = StateMemberRoll;
|
||||
const int POSITION_V_OFFSET = StateMemberVx;
|
||||
const int ORIENTATION_V_OFFSET = StateMemberVroll;
|
||||
const int POSITION_A_OFFSET = StateMemberAx;
|
||||
|
||||
//! @brief Pose and twist messages each
|
||||
//! contain six variables
|
||||
const int POSE_SIZE = 6;
|
||||
const int TWIST_SIZE = 6;
|
||||
const int POSITION_SIZE = 3;
|
||||
const int ORIENTATION_SIZE = 3;
|
||||
const int LINEAR_VELOCITY_SIZE = 3;
|
||||
const int ACCELERATION_SIZE = 3;
|
||||
|
||||
//! @brief Common variables
|
||||
const double PI = 3.141592653589793;
|
||||
const double TAU = 6.283185307179587;
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_FILTER_COMMON_H
|
||||
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* 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_FILTER_UTILITIES_H
|
||||
#define ROBOT_LOCALIZATION_FILTER_UTILITIES_H
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#define FB_DEBUG(msg) if (getDebug()) { *debugStream_ << msg; }
|
||||
|
||||
// Handy methods for debug output
|
||||
std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat);
|
||||
std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec);
|
||||
std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec);
|
||||
std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec);
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
namespace FilterUtilities
|
||||
{
|
||||
|
||||
//! @brief Utility method keeping RPY angles in the range [-pi, pi]
|
||||
//! @param[in] rotation - The rotation to bind
|
||||
//! @return the bounded value
|
||||
//!
|
||||
double clampRotation(double rotation);
|
||||
|
||||
//! @brief Utility method for appending tf2 prefixes cleanly
|
||||
//! @param[in] tfPrefix - the tf2 prefix to append
|
||||
//! @param[in, out] frameId - the resulting frame_id value
|
||||
//!
|
||||
void appendPrefix(std::string tfPrefix, std::string &frameId);
|
||||
|
||||
} // namespace FilterUtilities
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_FILTER_UTILITIES_H
|
||||
@@ -0,0 +1,216 @@
|
||||
/*
|
||||
* Copyright (C) 2010 Austin Robot Technology, and others
|
||||
* 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 names of the University of Texas at Austin, nor
|
||||
* Austin Robot Technology, nor the names of other 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 OWNER 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.
|
||||
*
|
||||
* This file contains code from multiple files in the original
|
||||
* source. The originals can be found here:
|
||||
*
|
||||
* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/UTM.h
|
||||
* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/conversions.h
|
||||
*/
|
||||
|
||||
#ifndef ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
|
||||
#define ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
|
||||
|
||||
/** @file
|
||||
|
||||
@brief Universal Transverse Mercator transforms.
|
||||
Functions to convert (spherical) latitude and longitude to and
|
||||
from (Euclidean) UTM coordinates.
|
||||
@author Chuck Gantz- chuck.gantz@globalstar.com
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <GeographicLib/MGRS.hpp>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
namespace NavsatConversions
|
||||
{
|
||||
|
||||
const double RADIANS_PER_DEGREE = M_PI/180.0;
|
||||
const double DEGREES_PER_RADIAN = 180.0/M_PI;
|
||||
|
||||
// Grid granularity for rounding UTM coordinates to generate MapXY.
|
||||
const double grid_size = 100000.0; // 100 km grid
|
||||
|
||||
// WGS84 Parameters
|
||||
#define WGS84_A 6378137.0 // major axis
|
||||
#define WGS84_B 6356752.31424518 // minor axis
|
||||
#define WGS84_F 0.0033528107 // ellipsoid flattening
|
||||
#define WGS84_E 0.0818191908 // first eccentricity
|
||||
#define WGS84_EP 0.0820944379 // second eccentricity
|
||||
|
||||
// UTM Parameters
|
||||
#define UTM_K0 0.9996 // scale factor
|
||||
#define UTM_FE 500000.0 // false easting
|
||||
#define UTM_FN_N 0.0 // false northing, northern hemisphere
|
||||
#define UTM_FN_S 10000000.0 // false northing, southern hemisphere
|
||||
#define UTM_E2 (WGS84_E*WGS84_E) // e^2
|
||||
#define UTM_E4 (UTM_E2*UTM_E2) // e^4
|
||||
#define UTM_E6 (UTM_E4*UTM_E2) // e^6
|
||||
#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2
|
||||
|
||||
/**
|
||||
* Utility function to convert geodetic to UTM position
|
||||
*
|
||||
* Units in are floating point degrees (sign for east/west)
|
||||
*
|
||||
* Units out are meters
|
||||
*
|
||||
* @todo deprecate this interface in favor of LLtoUTM()
|
||||
*/
|
||||
static inline void UTM(double lat, double lon, double *x, double *y)
|
||||
{
|
||||
// constants
|
||||
static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
|
||||
static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
|
||||
static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
|
||||
static const double m3 = -(35*UTM_E6/3072);
|
||||
|
||||
// compute the central meridian
|
||||
int cm = ((lon >= 0.0)
|
||||
? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)
|
||||
: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));
|
||||
|
||||
// convert degrees into radians
|
||||
double rlat = lat * RADIANS_PER_DEGREE;
|
||||
double rlon = lon * RADIANS_PER_DEGREE;
|
||||
double rlon0 = cm * RADIANS_PER_DEGREE;
|
||||
|
||||
// compute trigonometric functions
|
||||
double slat = sin(rlat);
|
||||
double clat = cos(rlat);
|
||||
double tlat = tan(rlat);
|
||||
|
||||
// decide the false northing at origin
|
||||
double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
|
||||
|
||||
double T = tlat * tlat;
|
||||
double C = UTM_EP2 * clat * clat;
|
||||
double A = (rlon - rlon0) * clat;
|
||||
double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
|
||||
+ m2*sin(4*rlat) + m3*sin(6*rlat));
|
||||
double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
|
||||
|
||||
// compute the easting-northing coordinates
|
||||
*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6
|
||||
+ (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);
|
||||
*y = fn + UTM_K0 * (M + V * tlat * (A*A/2
|
||||
+ (5-T+9*C+4*C*C)*pow(A, 4)/24
|
||||
+ ((61-58*T+T*T+600*C-330*UTM_EP2)
|
||||
* pow(A, 6)/720)));
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert lat/long to UTM coords.
|
||||
*
|
||||
* East Longitudes are positive, West longitudes are negative.
|
||||
* North latitudes are positive, South latitudes are negative
|
||||
* Lat and Long are in fractional degrees
|
||||
*
|
||||
* @param[out] gamma meridian convergence at point (degrees).
|
||||
*/
|
||||
static inline void LLtoUTM(const double Lat, const double Long,
|
||||
double &UTMNorthing, double &UTMEasting,
|
||||
std::string &UTMZone, double &gamma)
|
||||
{
|
||||
int zone;
|
||||
bool northp;
|
||||
double k_unused;
|
||||
GeographicLib::UTMUPS::Forward(Lat, Long, zone, northp, UTMEasting, UTMNorthing, gamma,
|
||||
k_unused, GeographicLib::UTMUPS::zonespec::MATCH);
|
||||
GeographicLib::MGRS::Forward(zone, northp, UTMEasting, UTMNorthing, -1, UTMZone);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert lat/long to UTM coords.
|
||||
*
|
||||
* East Longitudes are positive, West longitudes are negative.
|
||||
* North latitudes are positive, South latitudes are negative
|
||||
* Lat and Long are in fractional degrees
|
||||
*/
|
||||
static inline void LLtoUTM(const double Lat, const double Long,
|
||||
double &UTMNorthing, double &UTMEasting,
|
||||
std::string &UTMZone)
|
||||
{
|
||||
double gamma = 0.0;
|
||||
LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts UTM coords to lat/long.
|
||||
*
|
||||
* East Longitudes are positive, West longitudes are negative.
|
||||
* North latitudes are positive, South latitudes are negative
|
||||
* Lat and Long are in fractional degrees.
|
||||
*
|
||||
* @param[out] gamma meridian convergence at point (degrees).
|
||||
*/
|
||||
static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
|
||||
const std::string &UTMZone, double& Lat, double& Long,
|
||||
double& /*gamma*/)
|
||||
{
|
||||
int zone;
|
||||
bool northp;
|
||||
double x_unused;
|
||||
double y_unused;
|
||||
int prec_unused;
|
||||
GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true);
|
||||
GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts UTM coords to lat/long.
|
||||
*
|
||||
* East Longitudes are positive, West longitudes are negative.
|
||||
* North latitudes are positive, South latitudes are negative
|
||||
* Lat and Long are in fractional degrees.
|
||||
*/
|
||||
static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
|
||||
const std::string &UTMZone, double& Lat, double& Long)
|
||||
{
|
||||
double gamma;
|
||||
UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma);
|
||||
}
|
||||
|
||||
} // namespace NavsatConversions
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
|
||||
@@ -0,0 +1,382 @@
|
||||
/*
|
||||
* 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_NAVSAT_TRANSFORM_H
|
||||
#define ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H
|
||||
|
||||
#include <robot_localization/SetDatum.h>
|
||||
#include <robot_localization/ToLL.h>
|
||||
#include <robot_localization/FromLL.h>
|
||||
#include <robot_localization/SetUTMZone.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <GeographicLib/Geocentric.hpp>
|
||||
#include <GeographicLib/LocalCartesian.hpp>
|
||||
#include <GeographicLib/MGRS.hpp>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
class NavSatTransform
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor
|
||||
//!
|
||||
NavSatTransform(ros::NodeHandle nh, ros::NodeHandle nh_priv);
|
||||
|
||||
//! @brief Destructor
|
||||
//!
|
||||
~NavSatTransform();
|
||||
|
||||
private:
|
||||
//! @brief callback function which is called for periodic updates
|
||||
//!
|
||||
void periodicUpdate(const ros::TimerEvent& event);
|
||||
|
||||
//! @brief Computes the transform from the UTM frame to the odom frame
|
||||
//!
|
||||
void computeTransform();
|
||||
|
||||
//! @brief Callback for the datum service
|
||||
//!
|
||||
bool datumCallback(robot_localization::SetDatum::Request& request, robot_localization::SetDatum::Response&);
|
||||
|
||||
//! @brief Callback for the to Lat Long service
|
||||
//!
|
||||
bool toLLCallback(robot_localization::ToLL::Request& request, robot_localization::ToLL::Response& response);
|
||||
|
||||
//! @brief Callback for the from Lat Long service
|
||||
//!
|
||||
bool fromLLCallback(robot_localization::FromLL::Request& request, robot_localization::FromLL::Response& response);
|
||||
|
||||
//! @brief Callback for the UTM zone service
|
||||
//!
|
||||
bool setUTMZoneCallback(robot_localization::SetUTMZone::Request& request,
|
||||
robot_localization::SetUTMZone::Response& response);
|
||||
|
||||
//! @brief Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's
|
||||
//! centroid and returns the cartesian-frame pose of said centroid.
|
||||
//!
|
||||
void getRobotOriginCartesianPose(const tf2::Transform &gps_cartesian_pose,
|
||||
tf2::Transform &robot_cartesian_pose,
|
||||
const ros::Time &transform_time);
|
||||
|
||||
//! @brief Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid
|
||||
//! and returns the world-frame pose of said centroid.
|
||||
//!
|
||||
void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose,
|
||||
tf2::Transform &robot_odom_pose,
|
||||
const ros::Time &transform_time);
|
||||
|
||||
//! @brief Callback for the GPS fix data
|
||||
//! @param[in] msg The NavSatFix message to process
|
||||
//!
|
||||
void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg);
|
||||
|
||||
//! @brief Callback for the IMU data
|
||||
//! @param[in] msg The IMU message to process
|
||||
//!
|
||||
void imuCallback(const sensor_msgs::ImuConstPtr& msg);
|
||||
|
||||
//! @brief Callback for the odom data
|
||||
//! @param[in] msg The odometry message to process
|
||||
//!
|
||||
void odomCallback(const nav_msgs::OdometryConstPtr& msg);
|
||||
|
||||
//! @brief Converts the odometry data back to GPS and broadcasts it
|
||||
//! @param[out] filtered_gps The NavSatFix message to prepare
|
||||
//!
|
||||
bool prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps);
|
||||
|
||||
//! @brief Prepares the GPS odometry message before sending
|
||||
//! @param[out] gps_odom The odometry message to prepare
|
||||
//!
|
||||
bool prepareGpsOdometry(nav_msgs::Odometry &gps_odom);
|
||||
|
||||
//! @brief Used for setting the GPS data that will be used to compute the transform
|
||||
//! @param[in] msg The NavSatFix message to use in the transform
|
||||
//!
|
||||
void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg);
|
||||
|
||||
//! @brief Used for setting the odometry data that will be used to compute the transform
|
||||
//! @param[in] msg The odometry message to use in the transform
|
||||
//!
|
||||
void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg);
|
||||
|
||||
//! @brief Transforms the passed in pose from utm to map frame
|
||||
//! @param[in] cartesian_pose the pose in cartesian frame to use to transform
|
||||
//!
|
||||
nav_msgs::Odometry cartesianToMap(const tf2::Transform& cartesian_pose) const;
|
||||
|
||||
//! @brief Transforms the passed in point from map frame to lat/long
|
||||
//! @param[in] point the point in map frame to use to transform
|
||||
//!
|
||||
void mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const;
|
||||
|
||||
//! @brief Whether or not we broadcast the cartesian transform
|
||||
//!
|
||||
bool broadcast_cartesian_transform_;
|
||||
|
||||
//! @brief Whether to broadcast the cartesian transform as parent frame, default as child
|
||||
//!
|
||||
bool broadcast_cartesian_transform_as_parent_frame_;
|
||||
|
||||
//! @brief Whether or not we have new GPS data
|
||||
//!
|
||||
//! We only want to compute and broadcast our transformed GPS data if it's new. This variable keeps track of that.
|
||||
//!
|
||||
bool gps_updated_;
|
||||
|
||||
//! @brief Whether or not the GPS fix is usable
|
||||
//!
|
||||
bool has_transform_gps_;
|
||||
|
||||
//! @brief Signifies that we have received a usable IMU message
|
||||
//!
|
||||
bool has_transform_imu_;
|
||||
|
||||
//! @brief Signifies that we have received a usable odometry message
|
||||
//!
|
||||
bool has_transform_odom_;
|
||||
|
||||
//! @brief Whether or not we have new odometry data
|
||||
//!
|
||||
//! If we're creating filtered GPS messages, then we only want to broadcast them when new odometry data arrives.
|
||||
//!
|
||||
bool odom_updated_;
|
||||
|
||||
//! @brief Whether or not we publish filtered GPS messages
|
||||
//!
|
||||
bool publish_gps_;
|
||||
|
||||
//! @brief Whether or not we've computed a good heading
|
||||
//!
|
||||
bool transform_good_;
|
||||
|
||||
//! @brief Whether we get our datum from the first GPS message or from the set_datum
|
||||
//! service/parameter
|
||||
//!
|
||||
bool use_manual_datum_;
|
||||
|
||||
//! @brief Whether we get the transform's yaw from the odometry or IMU source
|
||||
//!
|
||||
bool use_odometry_yaw_;
|
||||
|
||||
//! @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian
|
||||
//!
|
||||
bool use_local_cartesian_;
|
||||
|
||||
//! @brief When true, do not print warnings for tf lookup failures.
|
||||
//!
|
||||
bool tf_silent_failure_;
|
||||
|
||||
//! @brief Local Cartesian projection around gps origin
|
||||
//!
|
||||
GeographicLib::LocalCartesian gps_local_cartesian_;
|
||||
|
||||
//! @brief Whether or not to report 0 altitude
|
||||
//!
|
||||
//! If this parameter is true, we always report 0 for the altitude of the converted GPS odometry message.
|
||||
//!
|
||||
bool zero_altitude_;
|
||||
|
||||
//! @brief Parameter that specifies the magnetic declination for the robot's environment.
|
||||
//!
|
||||
double magnetic_declination_;
|
||||
|
||||
//! @brief UTM's meridian convergence
|
||||
//!
|
||||
//! Angle between projected meridian (True North) and UTM's grid Y-axis.
|
||||
//! For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the equator and non-zero everywhere else.
|
||||
//! It increases as the poles are approached or as we're getting farther from central meridian.
|
||||
//!
|
||||
double utm_meridian_convergence_;
|
||||
|
||||
//! @brief IMU's yaw offset
|
||||
//!
|
||||
//! Your IMU should read 0 when facing *magnetic* north. If it doesn't, this (parameterized) value gives the offset
|
||||
//! (NOTE: if you have a magenetic declination, use the parameter setting for that).
|
||||
//!
|
||||
double yaw_offset_;
|
||||
|
||||
//! @brief Frame ID of the robot's body frame
|
||||
//!
|
||||
//! This is needed for obtaining transforms from the robot's body frame to the frames of sensors (IMU and GPS)
|
||||
//!
|
||||
std::string base_link_frame_id_;
|
||||
|
||||
//! @brief The cartesian frame ID, default as 'local_enu' if using Local Cartesian or 'utm' if using the UTM
|
||||
//! coordinates as our cartesian.
|
||||
//!
|
||||
std::string cartesian_frame_id_;
|
||||
|
||||
//! @brief The frame_id of the GPS message (specifies mounting location)
|
||||
//!
|
||||
std::string gps_frame_id_;
|
||||
|
||||
//! @brief the UTM zone (zero means UPS)
|
||||
//!
|
||||
int utm_zone_;
|
||||
|
||||
//! @brief hemisphere (true means north, false means south)
|
||||
//!
|
||||
bool northp_;
|
||||
|
||||
//! @brief Frame ID of the GPS odometry output
|
||||
//!
|
||||
//! This will just match whatever your odometry message has
|
||||
//!
|
||||
std::string world_frame_id_;
|
||||
|
||||
//! @brief Covariance for most recent odometry data
|
||||
//!
|
||||
Eigen::MatrixXd latest_odom_covariance_;
|
||||
|
||||
//! @brief Covariance for most recent GPS/UTM/LocalCartesian data
|
||||
//!
|
||||
Eigen::MatrixXd latest_cartesian_covariance_;
|
||||
|
||||
//! @brief Timestamp of the latest good GPS message
|
||||
//!
|
||||
//! We assign this value to the timestamp of the odometry message that we output
|
||||
//!
|
||||
ros::Time gps_update_time_;
|
||||
|
||||
//! @brief Timestamp of the latest good odometry message
|
||||
//!
|
||||
//! We assign this value to the timestamp of the odometry message that we output
|
||||
//!
|
||||
ros::Time odom_update_time_;
|
||||
|
||||
//! @brief Parameter that specifies the how long we wait for a transform to become available.
|
||||
//!
|
||||
ros::Duration transform_timeout_;
|
||||
|
||||
//! @brief timer calling periodicUpdate
|
||||
//!
|
||||
ros::Timer periodicUpdateTimer_;
|
||||
|
||||
//! @brief Latest IMU orientation
|
||||
//!
|
||||
tf2::Quaternion transform_orientation_;
|
||||
|
||||
//! @brief Latest GPS data, stored as Cartesian coords
|
||||
//!
|
||||
tf2::Transform latest_cartesian_pose_;
|
||||
|
||||
//! @brief Latest odometry pose data
|
||||
//!
|
||||
tf2::Transform latest_world_pose_;
|
||||
|
||||
//! @brief Holds the cartesian (UTM or local ENU) pose that is used to compute the transform
|
||||
//!
|
||||
tf2::Transform transform_cartesian_pose_;
|
||||
|
||||
//! @brief Latest IMU orientation
|
||||
//!
|
||||
tf2::Transform transform_world_pose_;
|
||||
|
||||
//! @brief Holds the Cartesian->odom transform
|
||||
//!
|
||||
tf2::Transform cartesian_world_transform_;
|
||||
|
||||
//! @brief Holds the odom->UTM transform for filtered GPS broadcast
|
||||
//!
|
||||
tf2::Transform cartesian_world_trans_inverse_;
|
||||
|
||||
//! @brief Publiser for filtered gps data
|
||||
//!
|
||||
ros::Publisher filtered_gps_pub_;
|
||||
|
||||
//! @brief GPS subscriber
|
||||
//!
|
||||
ros::Subscriber gps_sub_;
|
||||
|
||||
//! @brief Subscribes to imu topic
|
||||
//!
|
||||
ros::Subscriber imu_sub_;
|
||||
|
||||
//! @brief Odometry subscriber
|
||||
//!
|
||||
ros::Subscriber odom_sub_;
|
||||
|
||||
//! @brief Publisher for gps data
|
||||
//!
|
||||
ros::Publisher gps_odom_pub_;
|
||||
|
||||
//! @brief Service for set datum
|
||||
//!
|
||||
ros::ServiceServer datum_srv_;
|
||||
|
||||
//! @brief Service for to Lat Long
|
||||
//!
|
||||
ros::ServiceServer to_ll_srv_;
|
||||
|
||||
//! @brief Service for from Lat Long
|
||||
//!
|
||||
ros::ServiceServer from_ll_srv_;
|
||||
|
||||
//! @brief Service for set UTM zone
|
||||
//!
|
||||
ros::ServiceServer set_utm_zone_srv_;
|
||||
|
||||
//! @brief Transform buffer for managing coordinate transforms
|
||||
//!
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
|
||||
//! @brief Transform listener for receiving transforms
|
||||
//!
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
|
||||
//! @brief Used for publishing the static world_frame->cartesian transform
|
||||
//!
|
||||
tf2_ros::StaticTransformBroadcaster cartesian_broadcaster_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H
|
||||
@@ -0,0 +1,222 @@
|
||||
/*
|
||||
* Copyright (c) 2016, TNO IVS Helmond.
|
||||
* 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_ROBOT_LOCALIZATION_ESTIMATOR_H
|
||||
#define ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <boost/circular_buffer.hpp>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "robot_localization/filter_base.h"
|
||||
#include "robot_localization/filter_utilities.h"
|
||||
#include "robot_localization/filter_common.h"
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
struct Twist
|
||||
{
|
||||
Eigen::Vector3d linear;
|
||||
Eigen::Vector3d angular;
|
||||
};
|
||||
|
||||
//! @brief Robot Localization Estimator State
|
||||
//!
|
||||
//! The Estimator State data structure bundles the state information of the estimator.
|
||||
//!
|
||||
struct EstimatorState
|
||||
{
|
||||
EstimatorState():
|
||||
time_stamp(0.0),
|
||||
state(STATE_SIZE),
|
||||
covariance(STATE_SIZE, STATE_SIZE)
|
||||
{
|
||||
state.setZero();
|
||||
covariance.setZero();
|
||||
}
|
||||
|
||||
//! @brief Time at which this state is/was achieved
|
||||
double time_stamp;
|
||||
|
||||
//! @brief System state at time = time_stamp
|
||||
Eigen::VectorXd state;
|
||||
|
||||
//! @brief System state covariance at time = time_stamp
|
||||
Eigen::MatrixXd covariance;
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const EstimatorState& state)
|
||||
{
|
||||
return os << "state:\n - time_stamp: " << state.time_stamp <<
|
||||
"\n - state: \n" << state.state <<
|
||||
" - covariance: \n" << state.covariance;
|
||||
}
|
||||
};
|
||||
|
||||
namespace EstimatorResults
|
||||
{
|
||||
enum EstimatorResult
|
||||
{
|
||||
ExtrapolationIntoFuture = 0,
|
||||
Interpolation,
|
||||
ExtrapolationIntoPast,
|
||||
Exact,
|
||||
EmptyBuffer,
|
||||
Failed
|
||||
};
|
||||
} // namespace EstimatorResults
|
||||
typedef EstimatorResults::EstimatorResult EstimatorResult;
|
||||
|
||||
namespace FilterTypes
|
||||
{
|
||||
enum FilterType
|
||||
{
|
||||
EKF = 0,
|
||||
UKF,
|
||||
NotDefined
|
||||
};
|
||||
} // namespace FilterTypes
|
||||
typedef FilterTypes::FilterType FilterType;
|
||||
|
||||
//! @brief Robot Localization Listener class
|
||||
//!
|
||||
//! The Robot Localization Estimator class buffers states of and inputs to a system and can interpolate and extrapolate
|
||||
//! based on a given system model.
|
||||
//!
|
||||
class RobotLocalizationEstimator
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor for the RobotLocalizationListener class
|
||||
//!
|
||||
//! @param[in] args - Generic argument container (not used here, but needed so that the ROS filters can pass arbitrary
|
||||
//! arguments to templated filter types).
|
||||
//!
|
||||
explicit RobotLocalizationEstimator(unsigned int buffer_capacity,
|
||||
FilterType filter_type,
|
||||
const Eigen::MatrixXd& process_noise_covariance,
|
||||
const std::vector<double>& filter_args = std::vector<double>());
|
||||
|
||||
//! @brief Destructor for the RobotLocalizationListener class
|
||||
//!
|
||||
virtual ~RobotLocalizationEstimator();
|
||||
|
||||
//! @brief Sets the current internal state of the listener.
|
||||
//!
|
||||
//! @param[in] state - The new state vector to set the internal state to
|
||||
//!
|
||||
void setState(const EstimatorState& state);
|
||||
|
||||
//! @brief Returns the state at a given time
|
||||
//!
|
||||
//! Projects the current state and error matrices forward using a model of the robot's motion.
|
||||
//!
|
||||
//! @param[in] time - The time to which the prediction is being made
|
||||
//! @param[out] state - The returned state at the given time
|
||||
//!
|
||||
//! @return GetStateResult enum
|
||||
//!
|
||||
EstimatorResult getState(const double time, EstimatorState &state) const;
|
||||
|
||||
//! @brief Clears the internal state buffer
|
||||
//!
|
||||
void clearBuffer();
|
||||
|
||||
//! @brief Sets the buffer capacity
|
||||
//!
|
||||
//! @param[in] capacity - The new buffer capacity
|
||||
//!
|
||||
void setBufferCapacity(const int capacity);
|
||||
|
||||
//! @brief Returns the buffer capacity
|
||||
//!
|
||||
//! Returns the number of EstimatorState objects that can be pushed to the buffer before old ones are dropped. (The
|
||||
//! capacity of the buffer).
|
||||
//!
|
||||
//! @return buffer capacity
|
||||
//!
|
||||
unsigned int getBufferCapacity() const;
|
||||
|
||||
//! @brief Returns the current buffer size
|
||||
//!
|
||||
//! Returns the number of EstimatorState objects currently in the buffer.
|
||||
//!
|
||||
//! @return current buffer size
|
||||
//!
|
||||
unsigned int getSize() const;
|
||||
|
||||
private:
|
||||
friend std::ostream& operator<<(std::ostream &os, const RobotLocalizationEstimator& rle)
|
||||
{
|
||||
for ( boost::circular_buffer<EstimatorState>::const_iterator it = rle.state_buffer_.begin();
|
||||
it != rle.state_buffer_.end(); ++it )
|
||||
{
|
||||
os << *it << "\n";
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
//! @brief Extrapolates the given state to a requested time stamp
|
||||
//!
|
||||
//! @param[in] boundary_state - state from which to extrapolate
|
||||
//! @param[in] requested_time - time stamp to extrapolate to
|
||||
//! @param[out] state_at_req_time - predicted state at requested time
|
||||
//!
|
||||
void extrapolate(const EstimatorState& boundary_state,
|
||||
const double requested_time,
|
||||
EstimatorState& state_at_req_time) const;
|
||||
|
||||
//! @brief Interpolates the given state to a requested time stamp
|
||||
//!
|
||||
//! @param[in] given_state_1 - last state update before requested time
|
||||
//! @param[in] given_state_2 - next state update after requested time
|
||||
//! @param[in] requested_time - time stamp to extrapolate to
|
||||
//! @param[out] state_at_req_time - predicted state at requested time
|
||||
//!
|
||||
void interpolate(const EstimatorState& given_state_1, const EstimatorState& given_state_2,
|
||||
const double requested_time, EstimatorState& state_at_req_time) const;
|
||||
|
||||
//!
|
||||
//! @brief The buffer holding the system states that have come in. Interpolation and extrapolation is done starting
|
||||
//! from these states.
|
||||
//!
|
||||
boost::circular_buffer<EstimatorState> state_buffer_;
|
||||
|
||||
//!
|
||||
//! @brief A pointer to the filter instance that is used for extrapolation
|
||||
//!
|
||||
FilterBase* filter_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H
|
||||
@@ -0,0 +1,767 @@
|
||||
/*
|
||||
* 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 <robot_localization/ToggleFilterProcessing.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_srvs/Empty.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 <memory>
|
||||
#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 bool pose_use_child_frame,
|
||||
const double rejectionThreshold) :
|
||||
topicName_(topicName),
|
||||
updateVector_(updateVector),
|
||||
updateSum_(updateSum),
|
||||
differential_(differential),
|
||||
relative_(relative),
|
||||
pose_use_child_frame_(pose_use_child_frame),
|
||||
rejectionThreshold_(rejectionThreshold)
|
||||
{
|
||||
}
|
||||
|
||||
std::string topicName_;
|
||||
std::vector<int> updateVector_;
|
||||
int updateSum_;
|
||||
bool differential_;
|
||||
bool relative_;
|
||||
bool pose_use_child_frame_;
|
||||
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:
|
||||
//! @brief Constructor
|
||||
//!
|
||||
//! The RosFilter constructor makes sure that anyone using
|
||||
//! this template is doing so with the correct object type
|
||||
//!
|
||||
explicit RosFilter(ros::NodeHandle nh,
|
||||
ros::NodeHandle nh_priv,
|
||||
std::string node_name,
|
||||
std::vector<double> args = std::vector<double>());
|
||||
|
||||
//! @brief Constructor
|
||||
//!
|
||||
//! The RosFilter constructor makes sure that anyone using
|
||||
//! this template is doing so with the correct object type
|
||||
//!
|
||||
explicit RosFilter(ros::NodeHandle nh, ros::NodeHandle nh_priv, std::vector<double> args = std::vector<double>());
|
||||
|
||||
//! @brief Destructor
|
||||
//!
|
||||
//! Clears out the message filters and topic subscribers.
|
||||
//!
|
||||
~RosFilter();
|
||||
|
||||
//! @brief Initialize filter
|
||||
//
|
||||
void initialize();
|
||||
|
||||
//! @brief Resets the filter to its initial state
|
||||
//!
|
||||
void reset();
|
||||
|
||||
//! @brief Service callback to toggle processing measurements for a standby mode but continuing to publish
|
||||
//! @param[in] request - The state requested, on (True) or off (False)
|
||||
//! @param[out] response - status if upon success
|
||||
//! @return boolean true if successful, false if not
|
||||
bool toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request&,
|
||||
robot_localization::ToggleFilterProcessing::Response&);
|
||||
|
||||
//! @brief Callback method for receiving all acceleration (IMU) messages
|
||||
//! @param[in] msg - The ROS IMU message to take in.
|
||||
//! @param[in] callbackData - Relevant static callback data
|
||||
//! @param[in] targetFrame - The target frame_id into which to transform the data
|
||||
//!
|
||||
void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
|
||||
const CallbackData &callbackData,
|
||||
const std::string &targetFrame);
|
||||
|
||||
//! @brief Callback method for receiving non-stamped control input
|
||||
//! @param[in] msg - The ROS twist message to take in
|
||||
//!
|
||||
void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);
|
||||
|
||||
//! @brief Callback method for receiving stamped control input
|
||||
//! @param[in] msg - The ROS stamped twist message to take in
|
||||
//!
|
||||
void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);
|
||||
|
||||
//! @brief Adds a measurement to the queue of measurements to be processed
|
||||
//!
|
||||
//! @param[in] topicName - The name of the measurement source (only used for debugging)
|
||||
//! @param[in] measurement - The measurement to enqueue
|
||||
//! @param[in] measurementCovariance - The covariance of the measurement
|
||||
//! @param[in] updateVector - The boolean vector that specifies which variables to update from this measurement
|
||||
//! @param[in] mahalanobisThresh - Threshold, expressed as a Mahalanobis distance, for outlier rejection
|
||||
//! @param[in] time - The time of arrival (in seconds)
|
||||
//!
|
||||
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);
|
||||
|
||||
//! @brief Method for zeroing out 3D variables within measurements
|
||||
//! @param[out] measurement - The measurement whose 3D variables will be zeroed out
|
||||
//! @param[out] measurementCovariance - The covariance of the measurement
|
||||
//! @param[out] updateVector - The boolean update vector of the measurement
|
||||
//!
|
||||
//! If we're in 2D mode, then for every measurement from every sensor, we call this.
|
||||
//! It sets the 3D variables to 0, gives those variables tiny variances, and sets
|
||||
//! their updateVector values to 1.
|
||||
//!
|
||||
void forceTwoD(Eigen::VectorXd &measurement,
|
||||
Eigen::MatrixXd &measurementCovariance,
|
||||
std::vector<int> &updateVector);
|
||||
|
||||
//! @brief Retrieves the EKF's output for broadcasting
|
||||
//! @param[out] message - The standard ROS odometry message to be filled
|
||||
//! @return true if the filter is initialized, false otherwise
|
||||
//!
|
||||
bool getFilteredOdometryMessage(nav_msgs::Odometry &message);
|
||||
|
||||
//! @brief Retrieves the EKF's acceleration output for broadcasting
|
||||
//! @param[out] message - The standard ROS acceleration message to be filled
|
||||
//! @return true if the filter is initialized, false otherwise
|
||||
//!
|
||||
bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message);
|
||||
|
||||
//! @brief Callback method for receiving all IMU messages
|
||||
//! @param[in] msg - The ROS IMU message to take in.
|
||||
//! @param[in] topicName - The topic name for the IMU message (only used for debug output)
|
||||
//! @param[in] poseCallbackData - Relevant static callback data for orientation variables
|
||||
//! @param[in] twistCallbackData - Relevant static callback data for angular velocity variables
|
||||
//! @param[in] accelCallbackData - Relevant static callback data for linear acceleration variables
|
||||
//!
|
||||
//! This method separates out the orientation, angular velocity, and linear acceleration data and
|
||||
//! passed each on to its respective callback.
|
||||
//!
|
||||
void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,
|
||||
const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,
|
||||
const CallbackData &accelCallbackData);
|
||||
|
||||
//! @brief Processes all measurements in the measurement queue, in temporal order
|
||||
//!
|
||||
//! @param[in] currentTime - The time at which to carry out integration (the current time)
|
||||
//!
|
||||
void integrateMeasurements(const ros::Time ¤tTime);
|
||||
|
||||
//! @brief Differentiate angular velocity for angular acceleration
|
||||
//!
|
||||
//! @param[in] currentTime - The time at which to carry out differentiation (the current time)
|
||||
//!
|
||||
//! Maybe more state variables can be time-differentiated to estimate higher-order states,
|
||||
//! but now we only focus on obtaining the angular acceleration. It implements a backward-
|
||||
//! Euler differentiation.
|
||||
//!
|
||||
void differentiateMeasurements(const ros::Time ¤tTime);
|
||||
|
||||
//! @brief Loads all parameters from file
|
||||
//!
|
||||
void loadParams();
|
||||
|
||||
//! @brief Callback method for receiving all odometry messages
|
||||
//! @param[in] msg - The ROS odometry message to take in.
|
||||
//! @param[in] topicName - The topic name for the odometry message (only used for debug output)
|
||||
//! @param[in] poseCallbackData - Relevant static callback data for pose variables
|
||||
//! @param[in] twistCallbackData - Relevant static callback data for twist variables
|
||||
//!
|
||||
//! This method simply separates out the pose and twist data into two new messages, and passes them into their
|
||||
//! respective callbacks
|
||||
//!
|
||||
void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
|
||||
const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);
|
||||
|
||||
//! @brief Callback method for receiving all pose messages
|
||||
//! @param[in] msg - The ROS stamped pose with covariance message to take in
|
||||
//! @param[in] callbackData - Relevant static callback data
|
||||
//! @param[in] targetFrame - The target frame_id into which to transform the data
|
||||
//! @param[in] poseSourceFrame - The source frame_id from which to transform the data
|
||||
//! @param[in] imuData - Whether this data comes from an IMU
|
||||
//!
|
||||
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
|
||||
const CallbackData &callbackData,
|
||||
const std::string &targetFrame,
|
||||
const std::string &poseSourceFrame,
|
||||
const bool imuData);
|
||||
|
||||
//! @brief Callback method for manually setting/resetting the internal pose estimate
|
||||
//! @param[in] msg - The ROS stamped pose with covariance message to take in
|
||||
//!
|
||||
void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
|
||||
|
||||
//! @brief Service callback for manually setting/resetting the internal pose estimate
|
||||
//!
|
||||
//! @param[in] request - Custom service request with pose information
|
||||
//! @return true if successful, false if not
|
||||
bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
|
||||
robot_localization::SetPose::Response&);
|
||||
|
||||
//! @brief Service callback for manually enable the filter
|
||||
//! @param[in] request - N/A
|
||||
//! @param[out] response - N/A
|
||||
//! @return boolean true if successful, false if not
|
||||
bool enableFilterSrvCallback(std_srvs::Empty::Request&,
|
||||
std_srvs::Empty::Response&);
|
||||
|
||||
//! @brief Callback method for receiving all twist messages
|
||||
//! @param[in] msg - The ROS stamped twist with covariance message to take in.
|
||||
//! @param[in] callbackData - Relevant static callback data
|
||||
//! @param[in] targetFrame - The target frame_id into which to transform the data
|
||||
//!
|
||||
void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
|
||||
const CallbackData &callbackData,
|
||||
const std::string &targetFrame);
|
||||
|
||||
//! @brief Validates filter outputs for NaNs and Infinite values
|
||||
//! @param[out] message - The standard ROS odometry message to be validated
|
||||
//! @return true if the filter output is valid, false otherwise
|
||||
//!
|
||||
bool validateFilterOutput(const nav_msgs::Odometry &message);
|
||||
|
||||
protected:
|
||||
//! @brief Finds the latest filter state before the given timestamp and makes it the current state again.
|
||||
//!
|
||||
//! This method also inserts all measurements between the older filter timestamp and now into the measurements
|
||||
//! queue.
|
||||
//!
|
||||
//! @param[in] time - The time to which the filter state should revert
|
||||
//! @return True if restoring the filter succeeded. False if not.
|
||||
//!
|
||||
bool revertTo(const double time);
|
||||
|
||||
//! @brief Saves the current filter state in the queue of previous filter states
|
||||
//!
|
||||
//! These measurements will be used in backwards smoothing in the event that older measurements come in.
|
||||
//! @param[in] filter - The filter base object whose state we want to save
|
||||
//!
|
||||
void saveFilterState(FilterBase &filter);
|
||||
|
||||
//! @brief Removes measurements and filter states older than the given cutoff time.
|
||||
//! @param[in] cutoffTime - Measurements and states older than this time will be dropped.
|
||||
//!
|
||||
void clearExpiredHistory(const double cutoffTime);
|
||||
|
||||
//! @brief Clears measurement queue
|
||||
//!
|
||||
void clearMeasurementQueue();
|
||||
|
||||
//! @brief Adds a diagnostic message to the accumulating map and updates the error level
|
||||
//! @param[in] errLevel - The error level of the diagnostic
|
||||
//! @param[in] topicAndClass - The sensor topic (if relevant) and class of diagnostic
|
||||
//! @param[in] message - Details of the diagnostic
|
||||
//! @param[in] staticDiag - Whether or not this diagnostic information is static
|
||||
//!
|
||||
void addDiagnostic(const int errLevel,
|
||||
const std::string &topicAndClass,
|
||||
const std::string &message,
|
||||
const bool staticDiag);
|
||||
|
||||
//! @brief Aggregates all diagnostics so they can be published
|
||||
//! @param[in] wrapper - The diagnostic status wrapper to update
|
||||
//!
|
||||
void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper);
|
||||
|
||||
//! @brief Utility method for copying covariances from ROS covariance arrays
|
||||
//! to Eigen matrices
|
||||
//!
|
||||
//! This method copies the covariances and also does some data validation, which is
|
||||
//! why it requires more parameters than just the covariance containers.
|
||||
//! @param[in] arr - The source array for the covariance data
|
||||
//! @param[in] covariance - The destination matrix for the covariance data
|
||||
//! @param[in] topicName - The name of the source data topic (for debug purposes)
|
||||
//! @param[in] updateVector - The update vector for the source topic
|
||||
//! @param[in] offset - The "starting" location within the array/update vector
|
||||
//! @param[in] dimension - The number of values to copy, starting at the offset
|
||||
//!
|
||||
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);
|
||||
|
||||
//! @brief Utility method for copying covariances from Eigen matrices to ROS
|
||||
//! covariance arrays
|
||||
//!
|
||||
//! @param[in] covariance - The source matrix for the covariance data
|
||||
//! @param[in] arr - The destination array
|
||||
//! @param[in] dimension - The number of values to copy
|
||||
//!
|
||||
void copyCovariance(const Eigen::MatrixXd &covariance,
|
||||
double *arr,
|
||||
const size_t dimension);
|
||||
|
||||
//! @brief Loads fusion settings from the config file
|
||||
//! @param[in] topicName - The name of the topic for which to load settings
|
||||
//! @return The boolean vector of update settings for each variable for this topic
|
||||
//!
|
||||
std::vector<int> loadUpdateConfig(const std::string &topicName);
|
||||
|
||||
//! @brief Prepares an IMU message's linear acceleration for integration into the filter
|
||||
//! @param[in] msg - The IMU message to prepare
|
||||
//! @param[in] topicName - The name of the topic over which this message was received
|
||||
//! @param[in] targetFrame - The target tf frame
|
||||
//! @param[in] updateVector - The update vector for the data source
|
||||
//! @param[in] measurement - The twist data converted to a measurement
|
||||
//! @param[in] measurementCovariance - The covariance of the converted measurement
|
||||
//!
|
||||
bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
|
||||
const std::string &topicName,
|
||||
const std::string &targetFrame,
|
||||
const bool relative,
|
||||
std::vector<int> &updateVector,
|
||||
Eigen::VectorXd &measurement,
|
||||
Eigen::MatrixXd &measurementCovariance);
|
||||
|
||||
//! @brief Prepares a pose message for integration into the filter
|
||||
//! @param[in] msg - The pose message to prepare
|
||||
//! @param[in] topicName - The name of the topic over which this message was received
|
||||
//! @param[in] targetFrame - The target tf frame
|
||||
//! @param[in] sourceFrame - The source tf frame
|
||||
//! @param[in] differential - Whether we're carrying out differential integration
|
||||
//! @param[in] relative - Whether this measurement is processed relative to the first
|
||||
//! @param[in] imuData - Whether this measurement is from an IMU
|
||||
//! @param[in,out] updateVector - The update vector for the data source
|
||||
//! @param[out] measurement - The pose data converted to a measurement
|
||||
//! @param[out] measurementCovariance - The covariance of the converted measurement
|
||||
//! @return true indicates that the measurement was successfully prepared, false otherwise
|
||||
//!
|
||||
bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
|
||||
const std::string &topicName,
|
||||
const std::string &targetFrame,
|
||||
const std::string &sourceFrame,
|
||||
const bool differential,
|
||||
const bool relative,
|
||||
const bool imuData,
|
||||
std::vector<int> &updateVector,
|
||||
Eigen::VectorXd &measurement,
|
||||
Eigen::MatrixXd &measurementCovariance);
|
||||
|
||||
//! @brief Prepares a twist message for integration into the filter
|
||||
//! @param[in] msg - The twist message to prepare
|
||||
//! @param[in] topicName - The name of the topic over which this message was received
|
||||
//! @param[in] targetFrame - The target tf frame
|
||||
//! @param[in,out] updateVector - The update vector for the data source
|
||||
//! @param[out] measurement - The twist data converted to a measurement
|
||||
//! @param[out] measurementCovariance - The covariance of the converted measurement
|
||||
//! @return true indicates that the measurement was successfully prepared, false otherwise
|
||||
//!
|
||||
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);
|
||||
|
||||
|
||||
//! @brief callback function which is called for periodic updates
|
||||
//!
|
||||
void periodicUpdate(const ros::TimerEvent& event);
|
||||
|
||||
//! @brief Start the Filter disabled at startup
|
||||
//!
|
||||
//! If this is true, the filter reads parameters and prepares publishers and subscribes
|
||||
//! but does not integrate new messages into the state vector.
|
||||
//! The filter can be enabled later using a service.
|
||||
bool disabledAtStartup_;
|
||||
|
||||
//! @brief Whether the filter is enabled or not. See disabledAtStartup_.
|
||||
bool enabled_;
|
||||
|
||||
//! Whether we'll allow old measurements to cause a re-publication of the updated state
|
||||
bool permitCorrectedPublication_;
|
||||
|
||||
//! @brief By default, the filter predicts and corrects up to the time of the latest measurement. If this is set
|
||||
//! to true, the filter does the same, but then also predicts up to the current time step.
|
||||
//!
|
||||
bool predictToCurrentTime_;
|
||||
|
||||
//! @brief Whether or not we print diagnostic messages to the /diagnostics topic
|
||||
//!
|
||||
bool printDiagnostics_;
|
||||
|
||||
//! @brief Whether we publish the acceleration
|
||||
//!
|
||||
bool publishAcceleration_;
|
||||
|
||||
//! @brief Whether we publish the transform from the world_frame to the base_link_frame
|
||||
//!
|
||||
bool publishTransform_;
|
||||
|
||||
//! @brief Whether to reset the filters when backwards jump in time is detected
|
||||
//!
|
||||
//! This is usually the case when logs are being used and a jump in the logi
|
||||
//! is done or if a log files restarts from the beginning.
|
||||
//!
|
||||
bool resetOnTimeJump_;
|
||||
|
||||
//! @brief Whether or not we use smoothing
|
||||
//!
|
||||
bool smoothLaggedData_;
|
||||
|
||||
//! @brief Whether the filter should process new measurements or not.
|
||||
bool toggledOn_;
|
||||
|
||||
//! @brief Whether or not we're in 2D mode
|
||||
//!
|
||||
//! If this is true, the filter binds all 3D variables (Z,
|
||||
//! roll, pitch, and their respective velocities) to 0 for
|
||||
//! every measurement.
|
||||
//!
|
||||
bool twoDMode_;
|
||||
|
||||
//! @brief Whether or not we use a control term
|
||||
//!
|
||||
bool useControl_;
|
||||
|
||||
//! @brief When true, do not print warnings for tf lookup failures.
|
||||
//!
|
||||
bool tfSilentFailure_;
|
||||
|
||||
//! @brief The max (worst) dynamic diagnostic level.
|
||||
//!
|
||||
int dynamicDiagErrorLevel_;
|
||||
|
||||
//! @brief The max (worst) static diagnostic level.
|
||||
//!
|
||||
int staticDiagErrorLevel_;
|
||||
|
||||
//! @brief The frequency of the run loop
|
||||
//!
|
||||
double frequency_;
|
||||
|
||||
//! @brief What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
|
||||
//!
|
||||
double gravitationalAcc_;
|
||||
|
||||
//! @brief The depth of the history we track for smoothing/delayed measurement processing
|
||||
//!
|
||||
//! This is the guaranteed minimum buffer size for which previous states and measurements are kept.
|
||||
//!
|
||||
double historyLength_;
|
||||
|
||||
//! @brief minimal frequency
|
||||
//!
|
||||
double minFrequency_;
|
||||
|
||||
//! @brief maximal frequency
|
||||
double maxFrequency_;
|
||||
|
||||
//! @brief tf frame name for the robot's body frame
|
||||
//!
|
||||
std::string baseLinkFrameId_;
|
||||
|
||||
//! @brief tf frame name for the robot's body frame
|
||||
//!
|
||||
//! When the final state is computed, we "override" the output transform and message to have this frame for its
|
||||
//! child_frame_id. This helps to enable disconnected TF trees when multiple EKF instances are being run.
|
||||
//!
|
||||
std::string baseLinkOutputFrameId_;
|
||||
|
||||
//! @brief tf frame name for the robot's map (world-fixed) frame
|
||||
//!
|
||||
std::string mapFrameId_;
|
||||
|
||||
//! @brief tf frame name for the robot's odometry (world-fixed) frame
|
||||
//!
|
||||
std::string odomFrameId_;
|
||||
|
||||
//! @brief tf frame name that is the parent frame of the transform that this node will calculate and broadcast.
|
||||
//!
|
||||
std::string worldFrameId_;
|
||||
|
||||
//! @brief Used for outputting debug messages
|
||||
//!
|
||||
std::ofstream debugStream_;
|
||||
|
||||
//! @brief Contains the state vector variable names in string format
|
||||
//!
|
||||
std::vector<std::string> stateVariableNames_;
|
||||
|
||||
//! @brief Vector to hold our subscribers until they go out of scope
|
||||
//!
|
||||
std::vector<ros::Subscriber> topicSubs_;
|
||||
|
||||
//! @brief This object accumulates dynamic diagnostics, e.g., diagnostics relating
|
||||
//! to sensor data.
|
||||
//!
|
||||
//! The values are considered transient and are cleared at every iteration.
|
||||
//!
|
||||
std::map<std::string, std::string> dynamicDiagnostics_;
|
||||
|
||||
//! @brief Stores the first measurement from each topic for relative measurements
|
||||
//!
|
||||
//! When a given sensor is being integrated in relative mode, its first measurement
|
||||
//! is effectively treated as an offset, and future measurements have this first
|
||||
//! measurement removed before they are fused. This variable stores the initial
|
||||
//! measurements. Note that this is different from using differential mode, as in
|
||||
//! differential mode, pose data is converted to twist data, resulting in boundless
|
||||
//! error growth for the variables being fused. With relative measurements, the
|
||||
//! vehicle will start with a 0 heading and position, but the measurements are still
|
||||
//! fused absolutely.
|
||||
std::map<std::string, tf2::Transform> initialMeasurements_;
|
||||
|
||||
//! @brief Store the last time a message from each topic was received
|
||||
//!
|
||||
//! If we're getting messages rapidly, we may accidentally get
|
||||
//! an older message arriving after a newer one. This variable keeps
|
||||
//! track of the most recent message time for each subscribed message
|
||||
//! topic. We also use it when listening to odometry messages to
|
||||
//! determine if we should be using messages from that topic.
|
||||
//!
|
||||
std::map<std::string, ros::Time> lastMessageTimes_;
|
||||
|
||||
//! @brief We also need the previous covariance matrix for differential data
|
||||
//!
|
||||
std::map<std::string, Eigen::MatrixXd> previousMeasurementCovariances_;
|
||||
|
||||
//! @brief Stores the last measurement from a given topic for differential integration
|
||||
//!
|
||||
//! To carry out differential integration, we have to (1) transform
|
||||
//! that into the target frame (probably the frame specified by
|
||||
//! @p odomFrameId_), (2) "subtract" the previous measurement from
|
||||
//! the current measurement, and then (3) transform it again by the previous
|
||||
//! state estimate. This holds the measurements used for step (2).
|
||||
//!
|
||||
std::map<std::string, tf2::Transform> previousMeasurements_;
|
||||
|
||||
//! @brief If including acceleration for each IMU input, whether or not we remove acceleration due to gravity
|
||||
//!
|
||||
std::map<std::string, bool> removeGravitationalAcc_;
|
||||
|
||||
//! @brief This object accumulates static diagnostics, e.g., diagnostics relating
|
||||
//! to the configuration parameters.
|
||||
//!
|
||||
//! The values are treated as static and always reported (i.e., this object is never cleared)
|
||||
//!
|
||||
std::map<std::string, std::string> staticDiagnostics_;
|
||||
|
||||
//! @brief Last time mark that time-differentiation is calculated
|
||||
//!
|
||||
ros::Time lastDiffTime_;
|
||||
|
||||
//! @brief Last record of filtered angular velocity
|
||||
//!
|
||||
tf2::Vector3 lastStateTwistRot_;
|
||||
|
||||
//! @brief Calculated angular acceleration from time-differencing
|
||||
//!
|
||||
tf2::Vector3 angular_acceleration_;
|
||||
|
||||
//! @brief Covariance of the calculated angular acceleration
|
||||
//!
|
||||
Eigen::MatrixXd angular_acceleration_cov_;
|
||||
|
||||
//! @brief The most recent control input
|
||||
//!
|
||||
Eigen::VectorXd latestControl_;
|
||||
|
||||
//! @brief Message that contains our latest transform (i.e., state)
|
||||
//!
|
||||
//! We use the vehicle's latest state in a number of places, and often
|
||||
//! use it as a transform, so this is the most convenient variable to
|
||||
//! use as our global "current state" object
|
||||
//!
|
||||
geometry_msgs::TransformStamped worldBaseLinkTransMsg_;
|
||||
|
||||
//! @brief last call of periodicUpdate
|
||||
//!
|
||||
ros::Time lastDiagTime_;
|
||||
|
||||
//! @brief The time of the most recent published state
|
||||
//!
|
||||
ros::Time lastPublishedStamp_;
|
||||
|
||||
//! @brief Store the last time set pose message was received
|
||||
//!
|
||||
//! If we receive a setPose message to reset the filter, we can get in
|
||||
//! strange situations wherein we process the pose reset, but then even
|
||||
//! after another spin cycle or two, we can get a new message with a time
|
||||
//! stamp that is *older* than the setPose message's time stamp. This means
|
||||
//! we have to check the message's time stamp against the lastSetPoseTime_.
|
||||
ros::Time lastSetPoseTime_;
|
||||
|
||||
//! @brief The time of the most recent control input
|
||||
//!
|
||||
ros::Time latestControlTime_;
|
||||
|
||||
//! @brief For future (or past) dating the world_frame->base_link_frame transform
|
||||
//!
|
||||
ros::Duration tfTimeOffset_;
|
||||
|
||||
//! @brief Parameter that specifies the how long we wait for a transform to become available.
|
||||
//!
|
||||
ros::Duration tfTimeout_;
|
||||
|
||||
//! @brief Service that allows another node to toggle on/off filter processing while still publishing.
|
||||
//! Uses a robot_localization ToggleFilterProcessing service.
|
||||
//!
|
||||
ros::ServiceServer toggleFilterProcessingSrv_;
|
||||
|
||||
//! @brief timer calling periodicUpdate
|
||||
//!
|
||||
ros::Timer periodicUpdateTimer_;
|
||||
|
||||
//! @brief An implicitly time ordered queue of past filter states used for smoothing.
|
||||
//
|
||||
// front() refers to the filter state with the earliest timestamp.
|
||||
// back() refers to the filter state with the latest timestamp.
|
||||
FilterStateHistoryDeque filterStateHistory_;
|
||||
|
||||
//! @brief A deque of previous measurements which is implicitly ordered from earliest to latest measurement.
|
||||
// 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_;
|
||||
|
||||
//! @brief We process measurements by queueing them up in
|
||||
//! callbacks and processing them all at once within each
|
||||
//! iteration
|
||||
//!
|
||||
MeasurementQueue measurementQueue_;
|
||||
|
||||
//! @brief Our filter (EKF, UKF, etc.)
|
||||
//!
|
||||
T filter_;
|
||||
|
||||
//! @brief Node handle
|
||||
//!
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
//! @brief Local node handle (for params)
|
||||
//!
|
||||
ros::NodeHandle nhLocal_;
|
||||
|
||||
//! @brief optional acceleration publisher
|
||||
//!
|
||||
ros::Publisher accelPub_;
|
||||
|
||||
//! @brief position publisher
|
||||
//!
|
||||
ros::Publisher positionPub_;
|
||||
|
||||
//! @brief Subscribes to the control input topic
|
||||
//!
|
||||
ros::Subscriber controlSub_;
|
||||
|
||||
//! @brief Subscribes to the set_pose topic (usually published from rviz). Message
|
||||
//! type is geometry_msgs/PoseWithCovarianceStamped.
|
||||
//!
|
||||
ros::Subscriber setPoseSub_;
|
||||
|
||||
//! @brief Service that allows another node to enable the filter. Uses a standard Empty service.
|
||||
//!
|
||||
ros::ServiceServer enableFilterSrv_;
|
||||
|
||||
//! @brief Service that allows another node to change the current state and recieve a confirmation. Uses
|
||||
//! a custom SetPose service.
|
||||
//!
|
||||
ros::ServiceServer setPoseSrv_;
|
||||
|
||||
//! @brief Used for updating the diagnostics
|
||||
//!
|
||||
diagnostic_updater::Updater diagnosticUpdater_;
|
||||
|
||||
//! @brief Transform buffer for managing coordinate transforms
|
||||
//!
|
||||
tf2_ros::Buffer tfBuffer_;
|
||||
|
||||
//! @brief Transform listener for receiving transforms
|
||||
//!
|
||||
tf2_ros::TransformListener tfListener_;
|
||||
|
||||
//! @brief broadcaster of worldTransform tfs
|
||||
//!
|
||||
tf2_ros::TransformBroadcaster worldTransformBroadcaster_;
|
||||
|
||||
//! @brief optional signaling diagnostic frequency
|
||||
//!
|
||||
std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> freqDiag_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_ROS_FILTER_H
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* 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_TYPES_H
|
||||
#define ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H
|
||||
|
||||
#include "robot_localization/ros_filter.h"
|
||||
#include "robot_localization/ekf.h"
|
||||
#include "robot_localization/ukf.h"
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
typedef RosFilter<Ekf> RosEkf;
|
||||
typedef RosFilter<Ukf> RosUkf;
|
||||
|
||||
}
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* 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_UTILITIES_H
|
||||
#define ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
|
||||
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#define RF_DEBUG(msg) if (filter_.getDebug()) { debugStream_ << msg; }
|
||||
|
||||
// Handy methods for debug output
|
||||
std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec);
|
||||
std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat);
|
||||
std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans);
|
||||
std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec);
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
namespace RosFilterUtilities
|
||||
{
|
||||
|
||||
double getYaw(const tf2::Quaternion quat);
|
||||
|
||||
//! @brief Method for safely obtaining transforms.
|
||||
//! @param[in] buffer - tf buffer object to use for looking up the transform
|
||||
//! @param[in] targetFrame - The target frame of the desired transform
|
||||
//! @param[in] sourceFrame - The source frame of the desired transform
|
||||
//! @param[in] time - The time at which we want the transform
|
||||
//! @param[in] timeout - How long to block before falling back to last transform
|
||||
//! @param[out] targetFrameTrans - The resulting transform object
|
||||
//! @param[in] silent - Whether or not to print transform warnings
|
||||
//! @return Sets the value of @p targetFrameTrans and returns true if successful,
|
||||
//! false otherwise.
|
||||
//!
|
||||
//! This method attempts to obtain a transform from the @p sourceFrame to the @p
|
||||
//! targetFrame at the specific @p time. If no transform is available at that time,
|
||||
//! it attempts to simply obtain the latest transform. If that still fails, then the
|
||||
//! method checks to see if the transform is going from a given frame_id to itself.
|
||||
//! If any of these checks succeed, the method sets the value of @p targetFrameTrans
|
||||
//! and returns true, otherwise it returns false.
|
||||
//!
|
||||
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
|
||||
const std::string &targetFrame,
|
||||
const std::string &sourceFrame,
|
||||
const ros::Time &time,
|
||||
const ros::Duration &timeout,
|
||||
tf2::Transform &targetFrameTrans,
|
||||
const bool silent = false);
|
||||
|
||||
//! @brief Method for safely obtaining transforms.
|
||||
//! @param[in] buffer - tf buffer object to use for looking up the transform
|
||||
//! @param[in] targetFrame - The target frame of the desired transform
|
||||
//! @param[in] sourceFrame - The source frame of the desired transform
|
||||
//! @param[in] time - The time at which we want the transform
|
||||
//! @param[out] targetFrameTrans - The resulting transform object
|
||||
//! @param[in] silent - Whether or not to print transform warnings
|
||||
//! @return Sets the value of @p targetFrameTrans and returns true if successful,
|
||||
//! false otherwise.
|
||||
//!
|
||||
//! This method attempts to obtain a transform from the @p sourceFrame to the @p
|
||||
//! targetFrame at the specific @p time. If no transform is available at that time,
|
||||
//! it attempts to simply obtain the latest transform. If that still fails, then the
|
||||
//! method checks to see if the transform is going from a given frame_id to itself.
|
||||
//! If any of these checks succeed, the method sets the value of @p targetFrameTrans
|
||||
//! and returns true, otherwise it returns false.
|
||||
//!
|
||||
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
|
||||
const std::string &targetFrame,
|
||||
const std::string &sourceFrame,
|
||||
const ros::Time &time,
|
||||
tf2::Transform &targetFrameTrans,
|
||||
const bool silent = false);
|
||||
|
||||
//! @brief Utility method for converting quaternion to RPY
|
||||
//! @param[in] quat - The quaternion to convert
|
||||
//! @param[out] roll - The converted roll
|
||||
//! @param[out] pitch - The converted pitch
|
||||
//! @param[out] yaw - The converted yaw
|
||||
//!
|
||||
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw);
|
||||
|
||||
//! @brief Converts our Eigen state vector into a TF transform/pose
|
||||
//! @param[in] state - The state to convert
|
||||
//! @param[out] stateTF - The converted state
|
||||
//!
|
||||
void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF);
|
||||
|
||||
//! @brief Converts a TF transform/pose into our Eigen state vector
|
||||
//! @param[in] stateTF - The state to convert
|
||||
//! @param[out] state - The converted state
|
||||
//!
|
||||
void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state);
|
||||
|
||||
} // namespace RosFilterUtilities
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
|
||||
@@ -0,0 +1,171 @@
|
||||
/*
|
||||
* Copyright (c) 2016, TNO IVS Helmond.
|
||||
* 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_ROBOT_LOCALIZATION_LISTENER_H
|
||||
#define ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H
|
||||
|
||||
#include "robot_localization/robot_localization_estimator.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <geometry_msgs/AccelWithCovarianceStamped.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
//! @brief RosRobotLocalizationListener class
|
||||
//!
|
||||
//! This class wraps the RobotLocalizationEstimator. It listens to topics over which the (filtered) robot state is
|
||||
//! published (odom and accel) and pushes them into its instance of the RobotLocalizationEstimator. It exposes a
|
||||
//! getState method to offer the user the estimated state at a requested time. This class offers the option to run this
|
||||
//! listener without the need to run a separate node. If you do wish to run this functionality in a separate node,
|
||||
//! consider the robot localization listener node.
|
||||
//!
|
||||
class RosRobotLocalizationListener
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor
|
||||
//!
|
||||
//! The RosRobotLocalizationListener constructor initializes nodehandles, subscribers, a filter for synchronized
|
||||
//! listening to the topics it subscribes to, and an instance of the RobotLocalizationEstimator.
|
||||
//!
|
||||
RosRobotLocalizationListener();
|
||||
|
||||
//! @brief Destructor
|
||||
//!
|
||||
//! Empty destructor
|
||||
//!
|
||||
~RosRobotLocalizationListener();
|
||||
|
||||
//! @brief Get a state from the localization estimator
|
||||
//!
|
||||
//! Requests the predicted state and covariance at a given time from the Robot Localization Estimator.
|
||||
//!
|
||||
//! @param[in] time - time of the requested state
|
||||
//! @param[in] frame_id - frame id of which the state is requested.
|
||||
//! @param[out] state - state at the requested time
|
||||
//! @param[out] covariance - covariance at the requested time
|
||||
//!
|
||||
//! @return false if buffer is empty, true otherwise
|
||||
//!
|
||||
bool getState(const double time, const std::string& frame_id,
|
||||
Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
|
||||
std::string world_frame_id = "") const;
|
||||
|
||||
//! @brief Get a state from the localization estimator
|
||||
//!
|
||||
//! Overload of getState method for using ros::Time.
|
||||
//!
|
||||
//! @param[in] ros_time - ros time of the requested state
|
||||
//! @param[in] frame_id - frame id of which the state is requested.
|
||||
//! @param[out] state - state at the requested time
|
||||
//! @param[out] covariance - covariance at the requested time
|
||||
//!
|
||||
//! @return false if buffer is empty, true otherwise
|
||||
//!
|
||||
bool getState(const ros::Time& ros_time, const std::string& frame_id,
|
||||
Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
|
||||
const std::string& world_frame_id = "") const;
|
||||
|
||||
//!
|
||||
//! \brief getBaseFrameId Returns the base frame id of the localization listener
|
||||
//! \return The base frame id
|
||||
//!
|
||||
const std::string& getBaseFrameId() const;
|
||||
|
||||
//!
|
||||
//! \brief getWorldFrameId Returns the world frame id of the localization listener
|
||||
//! \return The world frame id
|
||||
//!
|
||||
const std::string& getWorldFrameId() const;
|
||||
|
||||
private:
|
||||
//! @brief Callback for odom and accel
|
||||
//!
|
||||
//! Puts the information from the odom and accel messages in a Robot Localization Estimator state and sets the most
|
||||
//! recent state of the estimator.
|
||||
//!
|
||||
//! @param[in] odometry message
|
||||
//! @param[in] accel message
|
||||
//!
|
||||
void odomAndAccelCallback(const nav_msgs::Odometry& odom, const geometry_msgs::AccelWithCovarianceStamped& accel);
|
||||
|
||||
//! @brief The core state estimator that facilitates inter- and
|
||||
//! extrapolation between buffered states.
|
||||
//!
|
||||
RobotLocalizationEstimator* estimator_;
|
||||
|
||||
//! @brief A public handle to the ROS node
|
||||
//!
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
//! @brief A private handle to the ROS node
|
||||
//!
|
||||
ros::NodeHandle nh_p_;
|
||||
|
||||
//! @brief Subscriber to the odometry state topic (output of a filter node)
|
||||
//!
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
|
||||
|
||||
//! @brief Subscriber to the acceleration state topic (output of a filter node)
|
||||
//!
|
||||
message_filters::Subscriber<geometry_msgs::AccelWithCovarianceStamped> accel_sub_;
|
||||
|
||||
//! @brief Waits for both an Odometry and an Accel message before calling a single callback function
|
||||
//!
|
||||
message_filters::TimeSynchronizer<nav_msgs::Odometry, geometry_msgs::AccelWithCovarianceStamped> sync_;
|
||||
|
||||
//! @brief Child frame id received from the Odometry message
|
||||
//!
|
||||
std::string base_frame_id_;
|
||||
|
||||
//! @brief Frame id received from the odometry message
|
||||
//!
|
||||
std::string world_frame_id_;
|
||||
|
||||
//! @brief Tf buffer for looking up transforms
|
||||
//!
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
|
||||
//! @brief Transform listener to fill the tf_buffer
|
||||
//!
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H
|
||||
@@ -0,0 +1,139 @@
|
||||
/*
|
||||
* 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_UKF_H
|
||||
#define ROBOT_LOCALIZATION_UKF_H
|
||||
|
||||
#include "robot_localization/filter_base.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <set>
|
||||
#include <queue>
|
||||
|
||||
namespace RobotLocalization
|
||||
{
|
||||
|
||||
//! @brief Unscented Kalman filter class
|
||||
//!
|
||||
//! Implementation of an unscenter Kalman filter (UKF). This
|
||||
//! class derives from FilterBase and overrides the predict()
|
||||
//! and correct() methods in keeping with the discrete time
|
||||
//! UKF algorithm. The algorithm was derived from the UKF
|
||||
//! Wikipedia article at
|
||||
//! (http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter)
|
||||
//! ...and this paper:
|
||||
//! J. J. LaViola, Jr., “A comparison of unscented and extended Kalman
|
||||
//! filtering for estimating quaternion motion,” in Proc. American Control
|
||||
//! Conf., Denver, CO, June 4–6, 2003, pp. 2435–2440
|
||||
//! Obtained here: http://www.cs.ucf.edu/~jjl/pubs/laviola_acc2003.pdf
|
||||
//!
|
||||
class Ukf: public FilterBase
|
||||
{
|
||||
public:
|
||||
//! @brief Constructor for the Ukf class
|
||||
//!
|
||||
//! @param[in] args - Generic argument container. It is assumed
|
||||
//! that args[0] constains the alpha parameter, args[1] contains
|
||||
//! the kappa parameter, and args[2] contains the beta parameter.
|
||||
//!
|
||||
explicit Ukf(std::vector<double> args);
|
||||
|
||||
//! @brief Destructor for the Ukf class
|
||||
//!
|
||||
~Ukf();
|
||||
|
||||
//! @brief Carries out the correct step in the predict/update cycle.
|
||||
//!
|
||||
//! @param[in] measurement - The measurement to fuse with our estimate
|
||||
//!
|
||||
void correct(const Measurement &measurement);
|
||||
|
||||
//! @brief Carries out the predict step in the predict/update cycle.
|
||||
//!
|
||||
//! Projects the state and error matrices forward using a model of
|
||||
//! the vehicle's motion.
|
||||
//!
|
||||
//! @param[in] referenceTime - The time at which the prediction is being made
|
||||
//! @param[in] delta - The time step over which to predict.
|
||||
//!
|
||||
void predict(const double referenceTime, const double delta);
|
||||
|
||||
protected:
|
||||
//! @brief Computes the weighted covariance and sigma points
|
||||
//!
|
||||
void generateSigmaPoints();
|
||||
|
||||
//! @brief Carries out the predict step for the posteriori state of a sigma
|
||||
//! point.
|
||||
//!
|
||||
//! Projects the state and error matrices forward using a model of
|
||||
//! the vehicle's motion.
|
||||
//!
|
||||
//! @param[in,out] sigmaPoint - The sigma point (state vector) to project
|
||||
//! @param[in] delta - The time step over which to project
|
||||
//!
|
||||
void projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta);
|
||||
|
||||
//! @brief The UKF sigma points
|
||||
//!
|
||||
//! Used to sample possible next states during prediction.
|
||||
//!
|
||||
std::vector<Eigen::VectorXd> sigmaPoints_;
|
||||
|
||||
//! @brief This matrix is used to generate the sigmaPoints_
|
||||
//!
|
||||
Eigen::MatrixXd weightedCovarSqrt_;
|
||||
|
||||
//! @brief The weights associated with each sigma point when generating
|
||||
//! a new state
|
||||
//!
|
||||
std::vector<double> stateWeights_;
|
||||
|
||||
//! @brief The weights associated with each sigma point when calculating
|
||||
//! a predicted estimateErrorCovariance_
|
||||
//!
|
||||
std::vector<double> covarWeights_;
|
||||
|
||||
//! @brief Used in weight generation for the sigma points
|
||||
//!
|
||||
double lambda_;
|
||||
|
||||
//! @brief Used to determine if we need to re-compute the sigma
|
||||
//! points when carrying out multiple corrections
|
||||
//!
|
||||
bool uncorrected_;
|
||||
};
|
||||
|
||||
} // namespace RobotLocalization
|
||||
|
||||
#endif // ROBOT_LOCALIZATION_UKF_H
|
||||
Reference in New Issue
Block a user