diff --git a/doc/state_estimation_nodes.rst b/doc/state_estimation_nodes.rst index e49892842..571951b3c 100644 --- a/doc/state_estimation_nodes.rst +++ b/doc/state_estimation_nodes.rst @@ -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. diff --git a/include/robot_localization/ros_filter.h b/include/robot_localization/ros_filter.h index 327ea50d7..5b96a249a 100644 --- a/include/robot_localization/ros_filter.h +++ b/include/robot_localization/ros_filter.h @@ -433,6 +433,9 @@ template 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. //! @@ -614,6 +617,10 @@ template 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 diff --git a/params/ekf_template.yaml b/params/ekf_template.yaml index d76851b68..06e722e7c 100644 --- a/params/ekf_template.yaml +++ b/params/ekf_template.yaml @@ -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 diff --git a/params/ukf_template.yaml b/params/ukf_template.yaml index 15410dfa8..683313ff5 100644 --- a/params/ukf_template.yaml +++ b/params/ukf_template.yaml @@ -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. @@ -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 diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 80850f49c..86200cdb5 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -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); @@ -996,12 +1000,12 @@ 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 << @@ -1009,8 +1013,9 @@ namespace RobotLocalization "\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", @@ -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()); @@ -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_) { @@ -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_) { @@ -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); }