Template Class RosFilter
Defined in File ros_filter.hpp
Inheritance Relationships
Base Type
public rclcpp::Node
Class Documentation
-
template<class T>
class RosFilter : public rclcpp::Node Public Functions
-
explicit RosFilter(const rclcpp::NodeOptions &options)
Constructor.
The RosFilter constructor makes sure that anyone using this template is doing so with the correct object type
-
~RosFilter()
Destructor.
Clears out the message filters and topic subscribers.
-
void reset()
Resets the filter to its initial state.
Service callback to toggle processing measurements for a standby mode but continuing to publish.
- Parameters:
request – [in] - The state requested, on (True) or off (False)
response – [out] - status if upon success
- Returns:
boolean true if successful, false if not
Callback method for receiving all acceleration (IMU) messages.
- Parameters:
msg – [in] - The ROS IMU message to take in.
callback_data – [in] - Relevant static callback data
target_frame – [in] - The target frame_id into which to transform the data
Callback method for receiving non-stamped control input.
- Parameters:
msg – [in] - The ROS twist message to take in
Callback method for receiving stamped control input.
- Parameters:
msg – [in] - The ROS stamped twist message to take in
-
void differentiateMeasurements(const rclcpp::Time ¤t_time)
Differentiate angular velocity for angular acceleration.
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.
- Parameters:
currentTime – [in] - The time at which to carry out differentiation (the current time)
-
void enqueueMeasurement(const std::string &topic_name, const Eigen::VectorXd &measurement, const Eigen::MatrixXd &measurement_covariance, const std::vector<bool> &update_vector, const double mahalanobis_thresh, const rclcpp::Time &time)
Adds a measurement to the queue of measurements to be processed.
- Parameters:
topic_name – [in] - The name of the measurement source (only used for debugging)
measurement – [in] - The measurement to enqueue
measurement_covariance – [in] - The covariance of the measurement
update_vector – [in] - The boolean vector that specifies which variables to update from this measurement
mahalanobis_thresh – [in] - Threshold, expressed as a Mahalanobis distance, for outlier rejection
time – [in] - The time of arrival (in seconds)
-
void forceTwoD(Eigen::VectorXd &measurement, Eigen::MatrixXd &measurement_covariance, std::vector<bool> &update_vector)
Method for zeroing out 3D variables within measurements.
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.
- Parameters:
measurement – [out] - The measurement whose 3D variables will be zeroed out
measurement_covariance – [out] - The covariance of the measurement
updateVector – [out] - The boolean update vector of the measurement
-
inline T &getFilter()
Method to get filter.
- Parameters:
filter – [out] - the underlying templated filter
Retrieves the EKF’s output for broadcasting.
- Parameters:
message – [out] - The standard ROS odometry message to be filled
- Returns:
true if the filter is initialized, false otherwise
-
bool getFilteredAccelMessage(geometry_msgs::msg::AccelWithCovarianceStamped *message)
Retrieves the EKF’s acceleration output for broadcasting.
- Parameters:
message – [out] - The standard ROS acceleration message to be filled
- Returns:
true if the filter is initialized, false otherwise
Callback method for receiving all IMU messages.
This method separates out the orientation, angular velocity, and linear acceleration data and passed each on to its respective callback.
- Parameters:
msg – [in] - The ROS IMU message to take in.
topic_name – [in] - The topic name for the IMU message (only used for debug output)
pose_callback_data – [in] - Relevant static callback data for orientation variables
twist_callback_data – [in] - Relevant static callback data for angular velocity variables
accel_callback_data – [in] - Relevant static callback data for linear acceleration variables
-
void integrateMeasurements(const rclcpp::Time ¤t_time)
Processes all measurements in the measurement queue, in temporal order.
- Parameters:
current_time – [in] - The time at which to carry out integration (the current time)
-
void loadParams()
Loads all parameters from file.
-
void periodicUpdate()
callback function which is called for periodic updates
Callback method for receiving all odometry messages.
This method simply separates out the pose and twist data into two new messages, and passes them into their respective callbacks
- Parameters:
msg – [in] - The ROS odometry message to take in.
topic_name – [in] - The topic name for the odometry message (only used for debug output)
pose_callback_data – [in] - Relevant static callback data for pose variables
twist_callback_data – [in] - Relevant static callback data for twist variables
Callback method for receiving all pose messages.
- Parameters:
msg – [in] - The ROS stamped pose with covariance message to take in
callback_data – [in] - Relevant static callback data
target_frame – [in] - The target frame_id into which to transform the data
pose_source_frame – [in] - The source frame_id from which to transform the data
imu_data – [in] - Whether this data comes from an IMU
-
void initialize()
initialize the filter
Service callback for resetting the filter to its initial state. Parameters are unused.
Callback method for manually setting/resetting the internal pose estimate.
- Parameters:
msg – [in] - The ROS stamped pose with covariance message to take in
Service callback for manually setting/resetting the internal pose estimate.
- Parameters:
request – [in] - Custom service request with pose information
- Returns:
true if successful, false if not
Service callback for manually enable the filter.
- Parameters:
request – [in] - N/A
response – [out] - N/A
- Returns:
boolean true if successful, false if not
Callback method for receiving all twist messages.
- Parameters:
msg – [in] - The ROS stamped twist with covariance message to take in.
callback_data – [in] - Relevant static callback data
target_frame – [in] - The target frame_id into which to transform the data
Validates filter outputs for NaNs and Infinite values.
- Parameters:
message – [out] - The standard ROS odometry message to be validated
- Returns:
true if the filter output is valid, false otherwise
Protected Functions
-
bool revertTo(const rclcpp::Time &time)
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.
- Parameters:
time – [in] - The time to which the filter state should revert
- Returns:
True if restoring the filter succeeded. False if not.
-
void saveFilterState(T &filter)
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.
- Parameters:
filter – [in] - The filter base object whose state we want to save
-
void clearExpiredHistory(const rclcpp::Time cutoff_time)
Removes measurements and filter states older than the given cutoff time.
- Parameters:
cutoff_time – [in] - Measurements and states older than this time will be dropped.
-
void clearMeasurementQueue()
Clears measurement queue.
-
void addDiagnostic(const int error_level, const std::string &topic_and_class, const std::string &message, const bool is_static)
Adds a diagnostic message to the accumulating map and updates the error level.
- Parameters:
error_level – [in] - The error level of the diagnostic
topic_and_class – [in] - The sensor topic (if relevant) and class of diagnostic
message – [in] - Details of the diagnostic
is_static – [in] - Whether or not this diagnostic information is static
-
void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper)
Aggregates all diagnostics so they can be published.
- Parameters:
wrapper – [in] - The diagnostic status wrapper to update
-
void copyCovariance(const double *covariance_in, Eigen::MatrixXd &covariance_out, const std::string &topic_name, const std::vector<bool> &update_vector, const size_t offset, const size_t dimension)
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.
- Parameters:
covariance_in – [in] - The source array for the covariance data
covariance_out – [in] - The destination matrix for the covariance data
topic_name – [in] - The name of the source data topic (for debug purposes)
update_vector – [in] - The update vector for the source topic
offset – [in] - The “starting” location within the array/update vector
dimension – [in] - The number of values to copy, starting at the offset
-
void copyCovariance(const Eigen::MatrixXd &covariance_in, double *covariance_out, const size_t dimension)
Utility method for copying covariances from Eigen matrices to ROS covariance arrays.
- Parameters:
covariance_in – [in] - The source matrix for the covariance data
covariance_out – [in] - The destination array
dimension – [in] - The number of values to copy
-
std::vector<bool> loadUpdateConfig(const std::string &topic_name)
Loads fusion settings from the config file.
- Parameters:
topic_name – [in] - The name of the topic for which to load settings
- Returns:
The boolean vector of update settings for each variable for this topic
Prepares an IMU message’s linear acceleration for integration into the filter.
- Parameters:
msg – [in] - The IMU message to prepare
topic_name – [in] - The name of the topic over which this message was received
target_frame – [in] - The target tf frame
relative – [in] - whether the IMU sensor reports pose relative to initialization pose
update_vector – [in] - The update vector for the data source
measurement – [in] - The twist data converted to a measurement
measurement_covariance – [in] - The covariance of the converted measurement
Prepares a pose message for integration into the filter.
- Parameters:
msg – [in] - The pose message to prepare
topic_name – [in] - The name of the topic over which this message was received
target_frame – [in] - The target tf frame
source_frame – [in] - The source tf frame
differential – [in] - Whether we’re carrying out differential integration
relative – [in] - Whether this measurement is processed relative to the first
imuData – [in] - Whether this measurement is from an IMU
updateVector – [inout] - The update vector for the data source
measurement – [out] - The pose data converted to a measurement
measurementCovariance – [out] - The covariance of the converted measurement
- Returns:
true indicates that the measurement was successfully prepared, false otherwise
Prepares a twist message for integration into the filter.
- Parameters:
msg – [in] - The twist message to prepare
topicName – [in] - The name of the topic over which this message was received
targetFrame – [in] - The target tf frame
updateVector – [inout] - The update vector for the data source
measurement – [out] - The twist data converted to a measurement
measurementCovariance – [out] - The covariance of the converted measurement
- Returns:
true indicates that the measurement was successfully prepared, false otherwise
Protected Attributes
-
bool print_diagnostics_
Whether or not we print diagnostic messages to the /diagnostics topic.
-
bool publish_acceleration_
Whether we publish the acceleration.
-
bool publish_transform_
Whether we publish the transform from the world_frame to the base_link_frame.
-
bool reset_on_time_jump_
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 smooth_lagged_data_
Whether or not we use smoothing.
-
bool toggled_on_
Whether the filter should process new measurements or not.
-
bool two_d_mode_
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 use_control_
Whether or not we use a control term.
-
bool stamped_control_
Whether or not we use a stamped control term.
-
bool disabled_at_startup_
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 enabled_
Whether the filter is enabled or not. See disabledAtStartup_.
-
bool permit_corrected_publication_
Whether we’ll allow old measurements to cause a re-publication of the updated state.
-
int dynamic_diag_error_level_
The max (worst) dynamic diagnostic level.
-
int static_diag_error_level_
The max (worst) static diagnostic level.
-
double frequency_
The frequency of the run loop.
-
double gravitational_acceleration_
What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
-
rclcpp::Duration history_length_
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.
-
rclcpp::Duration sensor_timeout_
The sensor timeout value that gets passed to the core filter.
-
std::string base_link_frame_id_
tf frame name for the robot’s body frame
-
std::string base_link_output_frame_id_
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 map_frame_id_
tf frame name for the robot’s map (world-fixed) frame
-
std::string odom_frame_id_
tf frame name for the robot’s odometry (world-fixed) frame
-
std::string world_frame_id_
tf frame name that is the parent frame of the transform that this node will calculate and broadcast.
-
std::ofstream debug_stream_
Used for outputting debug messages.
-
Eigen::VectorXd latest_control_
The most recent control input.
-
Eigen::MatrixXd process_noise_covariance_
The process noise covariance matrix that gets passed to the core filter.
-
Eigen::MatrixXd initial_estimate_error_covariance_
The initial estimate error covariance matrix that gets passed to the core filter.
-
geometry_msgs::msg::TransformStamped world_base_link_trans_msg_
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
-
rclcpp::Time last_diag_time_
last call of periodicUpdate
-
rclcpp::Time last_published_stamp_
The time of the most recent published state.
-
MeasurementQueue measurement_queue_
We process measurements by queueing them up in callbacks and processing them all at once within each iteration.
-
std::vector<std::string> state_variable_names_
Contains the state vector variable names in string format.
-
std::map<std::string, std::string> dynamic_diagnostics_
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> static_diagnostics_
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, rclcpp::Time> last_message_times_
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.
-
double last_diff_time_
Last time mark that time-differentiation is calculated, in seconds.
-
Eigen::MatrixXd angular_acceleration_cov_
Covariance of the calculated angular acceleration.
-
std::map<std::string, tf2::Transform> initial_measurements_
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, bool> remove_gravitational_acceleration_
If including acceleration for each IMU input, whether or not we remove acceleration due to gravity.
-
FilterStateHistoryDeque filter_state_history_
An implicitly time ordered queue of past filter states used for smoothing.
-
MeasurementHistoryDeque measurement_history_
A deque of previous measurements which is implicitly ordered from earliest to latest measurement.
-
std::vector<rclcpp::SubscriptionBase::SharedPtr> topic_subs_
Vector to hold our subscribers until they go out of scope.
-
std::map<std::string, tf2::Transform> previous_measurements_
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).
-
std::map<std::string, Eigen::MatrixXd> previous_measurement_covariances_
We also need the previous covariance matrix for differential data.
-
bool predict_to_current_time_
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.
-
rclcpp::Time last_set_pose_time_
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_.
-
rclcpp::Time latest_control_time_
The time of the most recent control input.
-
rclcpp::Duration tf_timeout_
Parameter that specifies the how long we wait for a transform to become available.
-
rclcpp::Duration tf_time_offset_
For future (or past) dating the world_frame->base_link_frame transform.
-
rclcpp::Service<robot_localization::srv::ToggleFilterProcessing>::SharedPtr toggle_filter_processing_srv_
Service that allows another node to toggle on/off filter processing while still publishing. Uses a robot_localization ToggleFilterProcessing service.
-
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr control_sub_
Subscribes to the control input topic.
-
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr stamped_control_sub_
Subscribes to the control stamped input topic.
-
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr set_pose_sub_
Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWithCovarianceStamped.
-
rclcpp::Service<robot_localization::srv::SetPose>::SharedPtr set_pose_service_
Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.
-
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr enable_filter_srv_
Service that allows another node to enable the filter. Uses a standard Empty service.
-
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv_
Service that resets the filter to its initial state.
-
std::unique_ptr<tf2_ros::Buffer> tf_buffer_
Transform buffer for managing coordinate transforms.
-
std::unique_ptr<tf2_ros::TransformListener> tf_listener_
Transform listener for receiving transforms.
-
std::shared_ptr<tf2_ros::TransformBroadcaster> world_transform_broadcaster_
broadcaster of worldTransform tfs
-
std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_
Used for updating the diagnostics.
-
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr position_pub_
Position publisher.
-
rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr accel_pub_
Acceleration publisher
-
rclcpp::TimerBase::SharedPtr timer_
Timer for filter updates.
-
std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> freq_diag_
optional signaling diagnostic frequency
-
double min_frequency_
minimum frequency threshold for frequency diagnostic Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam
-
double max_frequency_
maximum frequency threshold for frequency diagnostic Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam
-
explicit RosFilter(const rclcpp::NodeOptions &options)