#include <ros_filter.h>
Public Member Functions | |
| void | accelerationCallback (const sensor_msgs::Imu::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame) |
| Callback method for receiving all acceleration (IMU) messages. More... | |
| void | controlCallback (const geometry_msgs::Twist::ConstPtr &msg) |
| Callback method for receiving non-stamped control input. More... | |
| void | controlCallback (const geometry_msgs::TwistStamped::ConstPtr &msg) |
| Callback method for receiving stamped control input. More... | |
| bool | enableFilterSrvCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &) |
| Service callback for manually enable the filter. More... | |
| 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) |
| Adds a measurement to the queue of measurements to be processed. More... | |
| void | forceTwoD (Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance, std::vector< int > &updateVector) |
| Method for zeroing out 3D variables within measurements. More... | |
| bool | getFilteredAccelMessage (geometry_msgs::AccelWithCovarianceStamped &message) |
| Retrieves the EKF's acceleration output for broadcasting. More... | |
| bool | getFilteredOdometryMessage (nav_msgs::Odometry &message) |
| Retrieves the EKF's output for broadcasting. More... | |
| void | imuCallback (const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const CallbackData &poseCallbackData, const CallbackData &twistCallbackData, const CallbackData &accelCallbackData) |
| Callback method for receiving all IMU messages. More... | |
| void | integrateMeasurements (const ros::Time ¤tTime) |
| Processes all measurements in the measurement queue, in temporal order. More... | |
| void | loadParams () |
| Loads all parameters from file. More... | |
| void | odometryCallback (const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName, const CallbackData &poseCallbackData, const CallbackData &twistCallbackData) |
| Callback method for receiving all odometry messages. More... | |
| void | poseCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame, const bool imuData) |
| Callback method for receiving all pose messages. More... | |
| void | reset () |
| Resets the filter to its initial state. More... | |
| RosFilter (std::vector< double > args=std::vector< double >()) | |
| Constructor. More... | |
| void | run () |
| Main run method. More... | |
| void | setPoseCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) |
| Callback method for manually setting/resetting the internal pose estimate. More... | |
| bool | setPoseSrvCallback (robot_localization::SetPose::Request &request, robot_localization::SetPose::Response &) |
| Service callback for manually setting/resetting the internal pose estimate. More... | |
| bool | toggleFilterProcessingCallback (robot_localization::ToggleFilterProcessing::Request &, robot_localization::ToggleFilterProcessing::Response &) |
| Service callback to toggle processing measurements for a standby mode but continuing to publish. More... | |
| void | twistCallback (const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const CallbackData &callbackData, const std::string &targetFrame) |
| Callback method for receiving all twist messages. More... | |
| bool | validateFilterOutput (const nav_msgs::Odometry &message) |
| Validates filter outputs for NaNs and Infinite values. More... | |
| ~RosFilter () | |
| Destructor. More... | |
Protected Member Functions | |
| void | addDiagnostic (const int errLevel, const std::string &topicAndClass, const std::string &message, const bool staticDiag) |
| Adds a diagnostic message to the accumulating map and updates the error level. More... | |
| void | aggregateDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &wrapper) |
| Aggregates all diagnostics so they can be published. More... | |
| void | clearExpiredHistory (const double cutoffTime) |
| Removes measurements and filter states older than the given cutoff time. More... | |
| void | clearMeasurementQueue () |
| Clears measurement queue. More... | |
| 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) |
| Utility method for copying covariances from ROS covariance arrays to Eigen matrices. More... | |
| void | copyCovariance (const Eigen::MatrixXd &covariance, double *arr, const size_t dimension) |
| Utility method for copying covariances from Eigen matrices to ROS covariance arrays. More... | |
| std::vector< int > | loadUpdateConfig (const std::string &topicName) |
| Loads fusion settings from the config file. More... | |
| bool | prepareAcceleration (const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance) |
| Prepares an IMU message's linear acceleration for integration into the filter. More... | |
| bool | preparePose (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const bool differential, const bool relative, const bool imuData, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance) |
| Prepares a pose message for integration into the filter. More... | |
| 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) |
| Prepares a twist message for integration into the filter. More... | |
| bool | revertTo (const double time) |
| Finds the latest filter state before the given timestamp and makes it the current state again. More... | |
| void | saveFilterState (FilterBase &filter) |
| Saves the current filter state in the queue of previous filter states. More... | |
Protected Attributes | |
| std::string | baseLinkFrameId_ |
| tf frame name for the robot's body frame More... | |
| std::string | baseLinkOutputFrameId_ |
| tf frame name for the robot's body frame More... | |
| ros::Subscriber | controlSub_ |
| Subscribes to the control input topic. More... | |
| std::ofstream | debugStream_ |
| Used for outputting debug messages. More... | |
| diagnostic_updater::Updater | diagnosticUpdater_ |
| Used for updating the diagnostics. More... | |
| bool | disabledAtStartup_ |
| Start the Filter disabled at startup. More... | |
| int | dynamicDiagErrorLevel_ |
| The max (worst) dynamic diagnostic level. More... | |
| std::map< std::string, std::string > | dynamicDiagnostics_ |
| This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data. More... | |
| bool | enabled_ |
| Whether the filter is enabled or not. See disabledAtStartup_. More... | |
| ros::ServiceServer | enableFilterSrv_ |
| Service that allows another node to enable the filter. Uses a standard Empty service. More... | |
| T | filter_ |
| Our filter (EKF, UKF, etc.) More... | |
| FilterStateHistoryDeque | filterStateHistory_ |
| An implicitly time ordered queue of past filter states used for smoothing. More... | |
| double | frequency_ |
| The frequency of the run loop. More... | |
| double | gravitationalAcc_ |
| What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665. More... | |
| double | historyLength_ |
| The depth of the history we track for smoothing/delayed measurement processing. More... | |
| std::map< std::string, tf2::Transform > | initialMeasurements_ |
| Stores the first measurement from each topic for relative measurements. More... | |
| std::map< std::string, ros::Time > | lastMessageTimes_ |
| Store the last time a message from each topic was received. More... | |
| ros::Time | lastSetPoseTime_ |
| Store the last time set pose message was received. More... | |
| Eigen::VectorXd | latestControl_ |
| The most recent control input. More... | |
| ros::Time | latestControlTime_ |
| The time of the most recent control input. More... | |
| std::string | mapFrameId_ |
| tf frame name for the robot's map (world-fixed) frame More... | |
| MeasurementHistoryDeque | measurementHistory_ |
| A deque of previous measurements which is implicitly ordered from earliest to latest measurement. More... | |
| MeasurementQueue | measurementQueue_ |
| We process measurements by queueing them up in callbacks and processing them all at once within each iteration. More... | |
| ros::NodeHandle | nh_ |
| Node handle. More... | |
| ros::NodeHandle | nhLocal_ |
| Local node handle (for params) More... | |
| std::string | odomFrameId_ |
| tf frame name for the robot's odometry (world-fixed) frame More... | |
| bool | predictToCurrentTime_ |
| 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. More... | |
| std::map< std::string, Eigen::MatrixXd > | previousMeasurementCovariances_ |
| We also need the previous covariance matrix for differential data. More... | |
| std::map< std::string, tf2::Transform > | previousMeasurements_ |
| Stores the last measurement from a given topic for differential integration. More... | |
| bool | printDiagnostics_ |
| Whether or not we print diagnostic messages to the /diagnostics topic. More... | |
| bool | publishAcceleration_ |
| Whether we publish the acceleration. More... | |
| bool | publishTransform_ |
| Whether we publish the transform from the world_frame to the base_link_frame. More... | |
| std::map< std::string, bool > | removeGravitationalAcc_ |
| If including acceleration for each IMU input, whether or not we remove acceleration due to gravity. More... | |
| bool | resetOnTimeJump_ |
| Whether to reset the filters when backwards jump in time is detected. More... | |
| ros::ServiceServer | setPoseSrv_ |
| Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service. More... | |
| ros::Subscriber | setPoseSub_ |
| Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWithCovarianceStamped. More... | |
| bool | smoothLaggedData_ |
| Whether or not we use smoothing. More... | |
| std::vector< std::string > | stateVariableNames_ |
| Contains the state vector variable names in string format. More... | |
| int | staticDiagErrorLevel_ |
| The max (worst) static diagnostic level. More... | |
| std::map< std::string, std::string > | staticDiagnostics_ |
| This object accumulates static diagnostics, e.g., diagnostics relating to the configuration parameters. More... | |
| tf2_ros::Buffer | tfBuffer_ |
| Transform buffer for managing coordinate transforms. More... | |
| tf2_ros::TransformListener | tfListener_ |
| Transform listener for receiving transforms. More... | |
| ros::Duration | tfTimeOffset_ |
| For future (or past) dating the world_frame->base_link_frame transform. More... | |
| ros::Duration | tfTimeout_ |
| Parameter that specifies the how long we wait for a transform to become available. More... | |
| bool | toggledOn_ |
| $brief Whether the filter should process new measurements or not. More... | |
| ros::ServiceServer | toggleFilterProcessingSrv_ |
| Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service. More... | |
| std::vector< ros::Subscriber > | topicSubs_ |
| Vector to hold our subscribers until they go out of scope. More... | |
| bool | twoDMode_ |
| Whether or not we're in 2D mode. More... | |
| bool | useControl_ |
| Whether or not we use a control term. More... | |
| geometry_msgs::TransformStamped | worldBaseLinkTransMsg_ |
| Message that contains our latest transform (i.e., state) More... | |
| std::string | worldFrameId_ |
| tf frame name that is the parent frame of the transform that this node will calculate and broadcast. More... | |
Definition at line 106 of file ros_filter.h.
|
explicit |
Constructor.
The RosFilter constructor makes sure that anyone using this template is doing so with the correct object type
Definition at line 50 of file ros_filter.cpp.
| RobotLocalization::RosFilter< T >::~RosFilter | ( | ) |
Destructor.
Clears out the message filters and topic subscribers.
Definition at line 94 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::accelerationCallback | ( | const sensor_msgs::Imu::ConstPtr & | msg, |
| const CallbackData & | callbackData, | ||
| const std::string & | targetFrame | ||
| ) |
Callback method for receiving all acceleration (IMU) messages.
| [in] | msg | - The ROS IMU message to take in. |
| [in] | callbackData | - Relevant static callback data |
| [in] | targetFrame | - The target frame_id into which to transform the data |
Definition at line 149 of file ros_filter.cpp.
|
protected |
Adds a diagnostic message to the accumulating map and updates the error level.
| [in] | errLevel | - The error level of the diagnostic |
| [in] | topicAndClass | - The sensor topic (if relevant) and class of diagnostic |
| [in] | message | - Details of the diagnostic |
| [in] | staticDiag | - Whether or not this diagnostic information is static |
Definition at line 2161 of file ros_filter.cpp.
|
protected |
Aggregates all diagnostics so they can be published.
| [in] | wrapper | - The diagnostic status wrapper to update |
Definition at line 2179 of file ros_filter.cpp.
|
protected |
Removes measurements and filter states older than the given cutoff time.
| [in] | cutoffTime | - Measurements and states older than this time will be dropped. |
Definition at line 3165 of file ros_filter.cpp.
|
protected |
Clears measurement queue.
Definition at line 3191 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::controlCallback | ( | const geometry_msgs::Twist::ConstPtr & | msg | ) |
Callback method for receiving non-stamped control input.
| [in] | msg | - The ROS twist message to take in |
Definition at line 232 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::controlCallback | ( | const geometry_msgs::TwistStamped::ConstPtr & | msg | ) |
Callback method for receiving stamped control input.
| [in] | msg | - The ROS stamped twist message to take in |
Definition at line 242 of file ros_filter.cpp.
|
protected |
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.
| [in] | arr | - The source array for the covariance data |
| [in] | covariance | - The destination matrix for the covariance data |
| [in] | topicName | - The name of the source data topic (for debug purposes) |
| [in] | updateVector | - The update vector for the source topic |
| [in] | offset | - The "starting" location within the array/update vector |
| [in] | dimension | - The number of values to copy, starting at the offset |
Definition at line 2232 of file ros_filter.cpp.
|
protected |
Utility method for copying covariances from Eigen matrices to ROS covariance arrays.
| [in] | covariance | - The source matrix for the covariance data |
| [in] | arr | - The destination array |
| [in] | dimension | - The number of values to copy |
Definition at line 2295 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::enableFilterSrvCallback | ( | std_srvs::Empty::Request & | , |
| std_srvs::Empty::Response & | |||
| ) |
Service callback for manually enable the filter.
| [in] | request | - N/A |
| [out] | response | - N/A |
Definition at line 2053 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::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 | ||
| ) |
Adds a measurement to the queue of measurements to be processed.
| [in] | topicName | - The name of the measurement source (only used for debugging) |
| [in] | measurement | - The measurement to enqueue |
| [in] | measurementCovariance | - The covariance of the measurement |
| [in] | updateVector | - The boolean vector that specifies which variables to update from this measurement |
| [in] | mahalanobisThresh | - Threshold, expressed as a Mahalanobis distance, for outlier rejection |
| [in] | time | - The time of arrival (in seconds) |
Definition at line 265 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::forceTwoD | ( | Eigen::VectorXd & | measurement, |
| Eigen::MatrixXd & | measurementCovariance, | ||
| std::vector< int > & | updateVector | ||
| ) |
Method for zeroing out 3D variables within measurements.
| [out] | measurement | - The measurement whose 3D variables will be zeroed out |
| [out] | measurementCovariance | - The covariance of the measurement |
| [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.
Definition at line 286 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::getFilteredAccelMessage | ( | geometry_msgs::AccelWithCovarianceStamped & | message | ) |
Retrieves the EKF's acceleration output for broadcasting.
| [out] | message | - The standard ROS acceleration message to be filled |
Fill out the accel_msg
Definition at line 375 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::getFilteredOdometryMessage | ( | nav_msgs::Odometry & | message | ) |
Retrieves the EKF's output for broadcasting.
| [out] | message | - The standard ROS odometry message to be filled |
Definition at line 316 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::imuCallback | ( | const sensor_msgs::Imu::ConstPtr & | msg, |
| const std::string & | topicName, | ||
| const CallbackData & | poseCallbackData, | ||
| const CallbackData & | twistCallbackData, | ||
| const CallbackData & | accelCallbackData | ||
| ) |
Callback method for receiving all IMU messages.
| [in] | msg | - The ROS IMU message to take in. |
| [in] | topicName | - The topic name for the IMU message (only used for debug output) |
| [in] | poseCallbackData | - Relevant static callback data for orientation variables |
| [in] | twistCallbackData | - Relevant static callback data for angular velocity variables |
| [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.
Definition at line 410 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::integrateMeasurements | ( | const ros::Time & | currentTime | ) |
Processes all measurements in the measurement queue, in temporal order.
| [in] | currentTime | - The time at which to carry out integration (the current time) |
Definition at line 524 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::loadParams | ( | ) |
Loads all parameters from file.
Definition at line 644 of file ros_filter.cpp.
|
protected |
Loads fusion settings from the config file.
| [in] | topicName | - The name of the topic for which to load settings |
Definition at line 2309 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::odometryCallback | ( | const nav_msgs::Odometry::ConstPtr & | msg, |
| const std::string & | topicName, | ||
| const CallbackData & | poseCallbackData, | ||
| const CallbackData & | twistCallbackData | ||
| ) |
Callback method for receiving all odometry messages.
| [in] | msg | - The ROS odometry message to take in. |
| [in] | topicName | - The topic name for the odometry message (only used for debug output) |
| [in] | poseCallbackData | - Relevant static callback data for pose variables |
| [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
Definition at line 1619 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::poseCallback | ( | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & | msg, |
| const CallbackData & | callbackData, | ||
| const std::string & | targetFrame, | ||
| const bool | imuData | ||
| ) |
Callback method for receiving all pose messages.
| [in] | msg | - The ROS stamped pose with covariance message to take in |
| [in] | callbackData | - Relevant static callback data |
| [in] | targetFrame | - The target frame_id into which to transform the data |
| [in] | imuData | - Whether this data comes from an IMU |
Definition at line 1667 of file ros_filter.cpp.
|
protected |
Prepares an IMU message's linear acceleration for integration into the filter.
| [in] | msg | - The IMU message to prepare |
| [in] | topicName | - The name of the topic over which this message was received |
| [in] | targetFrame | - The target tf frame |
| [in] | updateVector | - The update vector for the data source |
| [in] | measurement | - The twist data converted to a measurement |
| [in] | measurementCovariance | - The covariance of the converted measurement |
Definition at line 2345 of file ros_filter.cpp.
|
protected |
Prepares a pose message for integration into the filter.
| [in] | msg | - The pose message to prepare |
| [in] | topicName | - The name of the topic over which this message was received |
| [in] | targetFrame | - The target tf frame |
| [in] | differential | - Whether we're carrying out differential integration |
| [in] | relative | - Whether this measurement is processed relative to the first |
| [in] | imuData | - Whether this measurement is from an IMU |
| [in,out] | updateVector | - The update vector for the data source |
| [out] | measurement | - The pose data converted to a measurement |
| [out] | measurementCovariance | - The covariance of the converted measurement |
Definition at line 2518 of file ros_filter.cpp.
|
protected |
Prepares a twist message for integration into the filter.
| [in] | msg | - The twist message to prepare |
| [in] | topicName | - The name of the topic over which this message was received |
| [in] | targetFrame | - The target tf frame |
| [in,out] | updateVector | - The update vector for the data source |
| [out] | measurement | - The twist data converted to a measurement |
| [out] | measurementCovariance | - The covariance of the converted measurement |
Definition at line 2911 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::reset | ( | ) |
Resets the filter to its initial state.
Definition at line 100 of file ros_filter.cpp.
|
protected |
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.
| [in] | time | - The time to which the filter state should revert |
Definition at line 3075 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::run | ( | ) |
Main run method.
Definition at line 1768 of file ros_filter.cpp.
|
protected |
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.
| [in] | filter | - The filter base object whose state we want to save |
Definition at line 3061 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::setPoseCallback | ( | const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & | msg | ) |
Callback method for manually setting/resetting the internal pose estimate.
| [in] | msg | - The ROS stamped pose with covariance message to take in |
Definition at line 1994 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::setPoseSrvCallback | ( | robot_localization::SetPose::Request & | request, |
| robot_localization::SetPose::Response & | |||
| ) |
Service callback for manually setting/resetting the internal pose estimate.
| [in] | request | - Custom service request with pose information |
Definition at line 2042 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::toggleFilterProcessingCallback | ( | robot_localization::ToggleFilterProcessing::Request & | req, |
| robot_localization::ToggleFilterProcessing::Response & | resp | ||
| ) |
Service callback to toggle processing measurements for a standby mode but continuing to publish.
| [in] | request | - The state requested, on (True) or off (False) |
| [out] | response | - status if upon success |
Definition at line 130 of file ros_filter.cpp.
| void RobotLocalization::RosFilter< T >::twistCallback | ( | const geometry_msgs::TwistWithCovarianceStamped::ConstPtr & | msg, |
| const CallbackData & | callbackData, | ||
| const std::string & | targetFrame | ||
| ) |
Callback method for receiving all twist messages.
| [in] | msg | - The ROS stamped twist with covariance message to take in. |
| [in] | callbackData | - Relevant static callback data |
| [in] | targetFrame | - The target frame_id into which to transform the data |
Definition at line 2071 of file ros_filter.cpp.
| bool RobotLocalization::RosFilter< T >::validateFilterOutput | ( | const nav_msgs::Odometry & | message | ) |
Validates filter outputs for NaNs and Infinite values.
| [out] | message | - The standard ROS odometry message to be validated |
Definition at line 3147 of file ros_filter.cpp.
|
protected |
tf frame name for the robot's body frame
All velocity and linear acceleration data is transformed into this frame before being fused
Definition at line 414 of file ros_filter.h.
|
protected |
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.
Definition at line 421 of file ros_filter.h.
|
protected |
Subscribes to the control input topic.
Definition at line 425 of file ros_filter.h.
|
protected |
Used for outputting debug messages.
Definition at line 443 of file ros_filter.h.
|
protected |
Used for updating the diagnostics.
Definition at line 451 of file ros_filter.h.
|
protected |
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.
Definition at line 634 of file ros_filter.h.
|
protected |
The max (worst) dynamic diagnostic level.
Definition at line 447 of file ros_filter.h.
|
protected |
This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data.
The values are considered transient and are cleared at every iteration.
Definition at line 439 of file ros_filter.h.
|
protected |
Whether the filter is enabled or not. See disabledAtStartup_.
Definition at line 637 of file ros_filter.h.
|
protected |
Service that allows another node to enable the filter. Uses a standard Empty service.
Definition at line 590 of file ros_filter.h.
|
protected |
Our filter (EKF, UKF, etc.)
Definition at line 455 of file ros_filter.h.
|
protected |
An implicitly time ordered queue of past filter states used for smoothing.
Definition at line 666 of file ros_filter.h.
|
protected |
The frequency of the run loop.
Definition at line 459 of file ros_filter.h.
|
protected |
What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
Definition at line 572 of file ros_filter.h.
|
protected |
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.
Definition at line 465 of file ros_filter.h.
|
protected |
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.
Definition at line 500 of file ros_filter.h.
|
protected |
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.
Definition at line 510 of file ros_filter.h.
|
protected |
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_.
Definition at line 519 of file ros_filter.h.
|
protected |
The most recent control input.
Definition at line 476 of file ros_filter.h.
|
protected |
The time of the most recent control input.
Definition at line 480 of file ros_filter.h.
|
protected |
tf frame name for the robot's map (world-fixed) frame
Definition at line 523 of file ros_filter.h.
|
protected |
A deque of previous measurements which is implicitly ordered from earliest to latest measurement.
Definition at line 672 of file ros_filter.h.
|
protected |
We process measurements by queueing them up in callbacks and processing them all at once within each iteration.
Definition at line 529 of file ros_filter.h.
|
protected |
Node handle.
Definition at line 533 of file ros_filter.h.
|
protected |
Local node handle (for params)
Definition at line 537 of file ros_filter.h.
|
protected |
tf frame name for the robot's odometry (world-fixed) frame
Definition at line 541 of file ros_filter.h.
|
protected |
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.
Definition at line 560 of file ros_filter.h.
|
protected |
We also need the previous covariance matrix for differential data.
Definition at line 555 of file ros_filter.h.
|
protected |
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 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).
Definition at line 551 of file ros_filter.h.
|
protected |
Whether or not we print diagnostic messages to the /diagnostics topic.
Definition at line 564 of file ros_filter.h.
|
protected |
Whether we publish the acceleration.
Definition at line 660 of file ros_filter.h.
|
protected |
Whether we publish the transform from the world_frame to the base_link_frame.
Definition at line 656 of file ros_filter.h.
|
protected |
If including acceleration for each IMU input, whether or not we remove acceleration due to gravity.
Definition at line 568 of file ros_filter.h.
|
protected |
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.
Definition at line 472 of file ros_filter.h.
|
protected |
Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.
Definition at line 582 of file ros_filter.h.
|
protected |
Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWithCovarianceStamped.
Definition at line 577 of file ros_filter.h.
|
protected |
Whether or not we use smoothing.
Definition at line 586 of file ros_filter.h.
|
protected |
Contains the state vector variable names in string format.
Definition at line 599 of file ros_filter.h.
|
protected |
The max (worst) static diagnostic level.
Definition at line 603 of file ros_filter.h.
|
protected |
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)
Definition at line 432 of file ros_filter.h.
|
protected |
Transform buffer for managing coordinate transforms.
Definition at line 607 of file ros_filter.h.
|
protected |
Transform listener for receiving transforms.
Definition at line 611 of file ros_filter.h.
|
protected |
For future (or past) dating the world_frame->base_link_frame transform.
Definition at line 615 of file ros_filter.h.
|
protected |
Parameter that specifies the how long we wait for a transform to become available.
Definition at line 484 of file ros_filter.h.
|
protected |
$brief Whether the filter should process new measurements or not.
Definition at line 640 of file ros_filter.h.
|
protected |
Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service.
Definition at line 595 of file ros_filter.h.
|
protected |
Vector to hold our subscribers until they go out of scope.
Definition at line 488 of file ros_filter.h.
|
protected |
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.
Definition at line 623 of file ros_filter.h.
|
protected |
Whether or not we use a control term.
Definition at line 627 of file ros_filter.h.
|
protected |
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
Definition at line 648 of file ros_filter.h.
|
protected |
tf frame name that is the parent frame of the transform that this node will calculate and broadcast.
Definition at line 652 of file ros_filter.h.