Merge pull request #523 from mabelzhang/dashing-port-melodic-2017-12-07

Port to Dashing: Bug fixes from melodic-devel Dec 07 2017

Author: SteveMacenski

include/robot_localization/ros_filter.hpp

@@ -625,6 +625,12 @@ class RosFilter : public rclcpp::Node
   //!
   std::map<std::string, Eigen::MatrixXd> previous_measurement_covariances_;
 
+  //! @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 predict_to_current_time_;
+
   //! @brief Store the last time set pose message was received
   //!
   //! If we receive a setPose message to reset the filter, we can get in

include/robot_localization/ros_filter_utilities.hpp

@@ -71,6 +71,7 @@ double getYaw(const tf2::Quaternion quat);
 //! @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.
 //!
@@ -87,14 +88,16 @@ bool lookupTransformSafe(
   const std::string & source_frame,
   const rclcpp::Time & time,
   const rclcpp::Duration & timeout,
-  tf2::Transform & target_frame_trans);
+  tf2::Transform & target_frame_trans,
+  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.
 //!
@@ -110,7 +113,8 @@ bool lookupTransformSafe(
   const std::string & targetFrame,
   const std::string & sourceFrame,
   const rclcpp::Time & time,
-  tf2::Transform & targetFrameTrans);
+  tf2::Transform & targetFrameTrans,
+  const bool silent = false);
 
 //! @brief Utility method for converting quaternion to RPY
 //! @param[in] quat - The quaternion to convert

src/filter_base.cpp

@@ -321,6 +321,7 @@ void FilterBase::setProcessNoiseCovariance(
   const Eigen::MatrixXd & process_noise_covariance)
 {
   process_noise_covariance_ = process_noise_covariance;
+  dynamic_process_noise_covariance_ = process_noise_covariance_;
 }
 
 void FilterBase::setSensorTimeout(const rclcpp::Duration & sensor_timeout)

src/navsat_transform.cpp

@@ -395,8 +395,11 @@ void NavSatTransform::getRobotOriginWorldPose(
       transform_timeout_, robot_orientation);
 
     if (can_transform) {
+      // Zero out rotation because we don't care about the orientation of the
+      // GPS receiver relative to base_link
       gps_offset_rotated.setOrigin(tf2::quatRotate(
           robot_orientation.getRotation(), gps_offset_rotated.getOrigin()));
+      gps_offset_rotated.setRotation(tf2::Quaternion::getIdentity());
       robot_odom_pose = gps_offset_rotated.inverse() * gps_odom_pose;
     } else {
       RCLCPP_ERROR(

src/ros_filter.cpp

@@ -536,6 +536,8 @@ void RosFilter<T>::integrateMeasurements(const rclcpp::Time & current_time)
     "\n" <<
     measurement_queue_.size() << " measurements in queue.\n");
 
+  bool predict_to_current_time = predict_to_current_time_;
+
   // If we have any measurements in the queue, process them
   if (!measurement_queue_.empty()) {
     // Check if the first measurement we're going to process is older than the
@@ -558,8 +560,9 @@ void RosFilter<T>::integrateMeasurements(const rclcpp::Time & current_time)
       if (!revertTo(first_measurement->time_ - rclcpp::Duration(1))) {
         RF_DEBUG("ERROR: history interval is too small to revert to time " <<
           filter_utilities::toSec(first_measurement->time_) << "\n");
-        // ROS_WARN_STREAM_THROTTLE(10.0, "Received old measurement for topic "
-        // << first_measurement->topic_name_ <<
+        // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_,
+        // "Received old measurement for topic "
+        //                         << first_measurement->topic_name_ <<
         //                         ", but history interval is insufficiently
         //                         sized to " "revert state and measurement
         //                         queue.");
@@ -624,23 +627,30 @@ void RosFilter<T>::integrateMeasurements(const rclcpp::Time & current_time)
 
     // If we get a large delta, then continuously predict until
     if (last_update_delta >= filter_.getSensorTimeout()) {
+      predict_to_current_time = true;
+
       RF_DEBUG("Sensor timeout! Last measurement time was " <<
         filter_utilities::toSec(filter_.getLastMeasurementTime()) <<
         ", current time is " << filter_utilities::toSec(current_time) <<
         ", delta is " << filter_utilities::toSec(last_update_delta) <<
         "\n");
-
-      filter_.validateDelta(last_update_delta);
-      filter_.predict(current_time, last_update_delta);
-
-      // Update the last measurement time and last update time
-      filter_.setLastMeasurementTime(filter_.getLastMeasurementTime() +
-        last_update_delta);
     }
   } else {
     RF_DEBUG("Filter not yet initialized.\n");
   }
 
+  if (filter_.getInitializedStatus() && predict_to_current_time) {
+    rclcpp::Duration last_update_delta =
+      current_time - filter_.getLastMeasurementTime();
+
+    filter_.validateDelta(last_update_delta);
+    filter_.predict(current_time, last_update_delta);
+
+    // Update the last measurement time and last update time
+    filter_.setLastMeasurementTime(filter_.getLastMeasurementTime() +
+      last_update_delta);
+  }
+
   RF_DEBUG("\n----- /RosFilter<T>::integrateMeasurements ------\n");
 }
 
@@ -1404,7 +1414,7 @@ void RosFilter<T>::loadParams()
           differential, relative, pose_mahalanobis_thresh);
         const CallbackData twist_callback_data(
           imu_topic_name + "_twist", twist_update_vec, twist_update_sum,
-          differential, relative, pose_mahalanobis_thresh);
+          differential, relative, twist_mahalanobis_thresh);
         const CallbackData accel_callback_data(
           imu_topic_name + "_acceleration", accel_update_vec, accelUpdateSum,
           differential, relative, accel_mahalanobis_thresh);
@@ -1919,9 +1929,12 @@ void RosFilter<T>::periodicUpdate()
     clearExpiredHistory(filter_.getLastMeasurementTime() - history_length_);
   }
 
-  if ((this->now() - cur_time).seconds() > 1. / frequency_) {
+  // Warn the user if the update took too long
+  const double loop_elapsed = (this->now() - cur_time).seconds();
+  if (loop_elapsed > 1. / frequency_) {
     std::cerr <<
-      "Failed to meet update rate! Try decreasing the rate, limiting "
+      "Failed to meet update rate! Took " << std::setprecision(20) <<
+      loop_elapsed << "seconds. Try decreasing the rate, limiting "
       "sensor output frequency, or limiting the number of sensors.\n";
   }
 }
@@ -2973,31 +2986,45 @@ bool RosFilter<T>::revertTo(const rclcpp::Time & time)
 
   // Walk back through the queue until we reach a filter state whose time stamp
   // is less than or equal to the requested time. Since every saved state after
-  // that time will be overwritten/corrected, we can pop from the queue.
+  // that time will be overwritten/corrected, we can pop from the queue. If the
+  // history is insufficiently short, we just take the oldest state we have.
+  FilterStatePtr last_history_state;
   while (!filter_state_history_.empty() &&
     filter_state_history_.back()->last_measurement_time_ > time)
   {
+    last_history_state = filter_state_history_.back();
     filter_state_history_.pop_back();
   }
 
-  // The state and measurement histories are stored at the same time, so if we
-  // have insufficient state history, we will also have insufficient measurement
-  // history.
-  if (filter_state_history_.empty()) {
+  // If the state history is not empty at this point, it means that our history
+  // was large enough, and we should revert to the state at the back of the
+  // history deque.
+  bool ret_val = false;
+  if (!filter_state_history_.empty()) {
+    ret_val = true;
+    last_history_state = filter_state_history_.back();
+  } else {
     RF_DEBUG("Insufficient history to revert to time " <<
       filter_utilities::toSec(time) << "\n");
 
-    return false;
+    if (last_history_state.get() != NULL) {
+      RF_DEBUG("Will revert to oldest state at " <<
+        filter_utilities::toSec(last_history_state->latest_control_time_) <<
+        ".\n");
+    }
   }
 
-  // Reset filter to the latest state from the queue.
-  const FilterStatePtr & state = filter_state_history_.back();
-  filter_.setState(state->state_);
-  filter_.setEstimateErrorCovariance(state->estimate_error_covariance_);
-  filter_.setLastMeasurementTime(state->last_measurement_time_);
+  // If we have a valid reversion state, revert
+  if (last_history_state.get() != NULL) {
+    // Reset filter to the latest state from the queue.
+    const FilterStatePtr & state = filter_state_history_.back();
+    filter_.setState(state->state_);
+    filter_.setEstimateErrorCovariance(state->estimate_error_covariance_);
+    filter_.setLastMeasurementTime(state->last_measurement_time_);
 
-  RF_DEBUG("Reverted to state with time " <<
-    filter_utilities::toSec(state->last_measurement_time_) << "\n");
+    RF_DEBUG("Reverted to state with time " <<
+      filter_utilities::toSec(state->last_measurement_time_) << "\n");
+  }
 
   // Repeat for measurements, but push every measurement onto the measurement
   // queue as we go
@@ -3014,7 +3041,7 @@ bool RosFilter<T>::revertTo(const rclcpp::Time & time)
 
   RF_DEBUG("\n----- /RosFilter<T>::revertTo\n");
 
-  return true;
+  return ret_val;
 }
 
 template<typename T>

src/ros_filter_utilities.cpp

@@ -41,6 +41,15 @@
 #include <string>
 #include <vector>
 
+#define THROTTLE(clock, duration, thing) do { \
+    static rclcpp::Time _last_output_time ## __LINE__(0, 0, (clock)->get_clock_type()); \
+    auto _now = (clock)->now(); \
+    if (_now - _last_output_time ## __LINE__ > (duration)) { \
+      _last_output_time ## __LINE__ = _now; \
+      thing; \
+    } \
+} while (0)
+
 std::ostream & operator<<(std::ostream & os, const tf2::Vector3 & vec)
 {
   os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " <<
@@ -117,7 +126,8 @@ bool lookupTransformSafe(
   const std::string & source_frame,
   const rclcpp::Time & time,
   const rclcpp::Duration & duration,
-  tf2::Transform & target_frame_trans)
+  tf2::Transform & target_frame_trans,
+  const bool silent)
 {
   bool retVal = true;
   tf2::TimePoint time_tf = tf2::timeFromSec(filter_utilities::toSec(time));
@@ -140,15 +150,19 @@ bool lookupTransformSafe(
         .transform,
         target_frame_trans);
 
-      // ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << source_frame << " to
-      // " << target_frame <<
-      //                              " was unavailable for the time requested.
-      //                              Using latest instead.\n");
+      if (!silent) {
+        // ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << source_frame <<
+        // " to " << target_frame <<
+        //                              " was unavailable for the time
+        //                              requested. Using latest instead.\n");
+      }
     } catch (tf2::TransformException & ex) {
-      // ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " <<
-      // source_frame <<
-      //                              " to " << target_frame << ". Error was "
-      //                              << ex.what() << "\n");
+      if (!silent) {
+        // ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " <<
+        // source_frame <<
+        //                              " to " << target_frame << ". Error was "
+        //                              << ex.what() << "\n");
+      }
 
       retVal = false;
     }
@@ -172,10 +186,11 @@ bool lookupTransformSafe(
   const std::string & target_frame,
   const std::string & source_frame,
   const rclcpp::Time & time,
-  tf2::Transform & target_frame_trans)
+  tf2::Transform & target_frame_trans,
+  const bool silent)
 {
   return lookupTransformSafe(buffer, target_frame, source_frame, time,
-           rclcpp::Duration(0), target_frame_trans);
+           rclcpp::Duration(0), target_frame_trans, silent);
 }
 
 void quatToRPY(

test/test_ukf.cpp

@@ -63,6 +63,8 @@ TEST(UkfTest, Measurements) {
 
   filter->getFilter().setEstimateErrorCovariance(initialCovar);
 
+  EXPECT_EQ(filter->getFilter().getEstimateErrorCovariance(), initialCovar);
+
   Eigen::VectorXd measurement(STATE_SIZE);
   measurement.setIdentity();