Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Melodic backport of https://github.com/cra-ros-pkg/robot_localization/pull/595 #840

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

Melodic backport of https://github.com/cra-ros-pkg/robot_localization/pull/595 #840

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions doc/state_estimation_nodes.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,10 @@ If *true*, the state estimation node will publish the transform from the frame s
^^^^^^^^^^^^^^^^^^^^^
If *true*, the state estimation node will publish the linear acceleration state. Defaults to *false*.

~permit_corrected_publication
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
When the state estimation nodes publish the state at time `t`, but then receive a measurement with a timestamp < `t`, they re-publish the corrected state, with the same time stamp as the previous publication. Setting this parameter to *false* disables that behavior. Defaults to *false*.

~print_diagnostics
^^^^^^^^^^^^^^^^^^
If true, the state estimation node will publish diagnostic messages to the ``/diagnostics`` topic. This is useful for debugging your configuration and sensor data.
Expand Down
7 changes: 7 additions & 0 deletions include/robot_localization/ros_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -433,6 +433,9 @@ template<class T> class RosFilter
//! @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.
//!
Expand Down Expand Up @@ -614,6 +617,10 @@ template<class T> class RosFilter
//!
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
Expand Down
3 changes: 3 additions & 0 deletions params/ekf_template.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,9 @@ publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false

# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false

# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
Expand Down
12 changes: 12 additions & 0 deletions params/ukf_template.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@ transform_timeout: 0.0
# unhappy with any settings or data.
print_diagnostics: true

# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false

# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
Expand All @@ -35,6 +38,15 @@ debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt

# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true

# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false

# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false

# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
Expand Down
38 changes: 29 additions & 9 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <algorithm>
#include <iostream>
#include <map>
#include <memory>
#include <string>
Expand Down Expand Up @@ -824,6 +825,9 @@ namespace RobotLocalization
// Whether we're publishing the acceleration state transform
nhLocal_.param("publish_acceleration", publishAcceleration_, false);

// Whether we'll allow old measurements to cause a re-publication of the updated state
nhLocal_.param("permit_corrected_publication", permitCorrectedPublication_, false);

// Transform future dating
double offsetTmp;
nhLocal_.param("transform_time_offset", offsetTmp, 0.0);
Expand Down Expand Up @@ -996,21 +1000,22 @@ namespace RobotLocalization
"\ntransform_timeout is " << tfTimeout_.toSec() <<
"\nfrequency is " << frequency_ <<
"\nsensor_timeout is " << filter_.getSensorTimeout() <<
"\ntwo_d_mode is " << (twoDMode_ ? "true" : "false") <<
"\nsilent_tf_failure is " << (silentTfFailure_ ? "true" : "false") <<
"\nsmooth_lagged_data is " << (smoothLaggedData_ ? "true" : "false") <<
"\ntwo_d_mode is " << std::boolalpha << twoDMode_ <<
"\nsmooth_lagged_data is " << std::boolalpha << smoothLaggedData_ <<
"\nhistory_length is " << historyLength_ <<
"\nuse_control is " << (useControl_ ? "true" : "false") <<
"\nstamped_control is " << (stampedControl ? "true" : "false") <<
"\nuse_control is " << std::boolalpha << useControl_ <<
"\nstamped_control is " << std::boolalpha << stampedControl <<
"\ncontrol_config is " << controlUpdateVector <<
"\ncontrol_timeout is " << controlTimeout <<
"\nacceleration_limits are " << accelerationLimits <<
"\nacceleration_gains are " << accelerationGains <<
"\ndeceleration_limits are " << decelerationLimits <<
"\ndeceleration_gains are " << decelerationGains <<
"\ninitial state is " << filter_.getState() <<
"\ndynamic_process_noise_covariance is " << (dynamicProcessNoiseCovariance ? "true" : "false") <<
"\nprint_diagnostics is " << (printDiagnostics_ ? "true" : "false") << "\n");
"\ndynamic_process_noise_covariance is " << std::boolalpha << dynamicProcessNoiseCovariance <<
"\npermit_corrected_publication is " << std::boolalpha << permitCorrectedPublication_ <<
"\nprint_diagnostics is " << std::boolalpha << printDiagnostics_ << "\n");

// Create a subscriber for manually setting/resetting pose
setPoseSub_ = nh_.subscribe("set_pose",
Expand Down Expand Up @@ -1898,6 +1903,8 @@ namespace RobotLocalization
// Get latest state and publish it
nav_msgs::Odometry filteredPosition;

bool corrected_data = false;

if (getFilteredOdometryMessage(filteredPosition))
{
worldBaseLinkTransMsg_.transform = tf2::toMsg(tf2::Transform::getIdentity());
Expand All @@ -1918,9 +1925,16 @@ namespace RobotLocalization
" This was likely due to poorly coniditioned process, noise, or sensor covariances.");
}

// If we're trying to publish with the same time stamp, it means that we had a measurement get inserted into the
// filter history, and our state estimate was updated after it was already published. As of ROS Noetic, TF2 will
// issue warnings whenever this occurs, so we make this behavior optional.
// Just for safety, we also check for the condition where the last published stamp is *later* than this stamp.
// This should never happen, but we should handle the case anyway.
corrected_data = (!permitCorrectedPublication_ && lastPublishedStamp_ >= filteredPosition.header.stamp);

// If the worldFrameId_ is the odomFrameId_ frame, then we can just send the transform. If the
// worldFrameId_ is the mapFrameId_ frame, we'll have some work to do.
if (publishTransform_)
if (publishTransform_ && !corrected_data)
{
if (filteredPosition.header.frame_id == odomFrameId_)
{
Expand Down Expand Up @@ -1980,7 +1994,13 @@ namespace RobotLocalization
}

// Fire off the position and the transform
positionPub_.publish(filteredPosition);
if (!corrected_data)
{
positionPub_.publish(filteredPosition);
}

// Retain the last published stamp so we can detect repeated transforms in future cycles
lastPublishedStamp_ = filteredPosition.header.stamp;

if (printDiagnostics_)
{
Expand All @@ -1990,7 +2010,7 @@ namespace RobotLocalization

// Publish the acceleration if desired and filter is initialized
geometry_msgs::AccelWithCovarianceStamped filteredAcceleration;
if (publishAcceleration_ && getFilteredAccelMessage(filteredAcceleration))
if (publishAcceleration_ && getFilteredAccelMessage(filteredAcceleration) && !corrected_data)
{
accelPub_.publish(filteredAcceleration);
}
Expand Down