Port PR #753 and #728 to ROS2 version (#765)

* compiling version of commit #753 and #728 ported to ros2 rolling

* format fixes

* fix time source disagreement by converting to seconds beforehand, append parameter usage, fix linting

* fix linting and uncrustify

Author: HaoguangYang

include/robot_localization/ros_filter.hpp

@@ -72,16 +72,19 @@ struct CallbackData
     const std::string & topic_name,
     const std::vector<bool> & update_vector, const int update_sum,
     const bool differential, const bool relative,
+    const bool pose_use_child_frame,
     const double rejection_threshold)
   : topic_name_(topic_name), update_vector_(update_vector),
     update_sum_(update_sum), differential_(differential),
-    relative_(relative), rejection_threshold_(rejection_threshold) {}
+    relative_(relative), pose_use_child_frame_(pose_use_child_frame),
+    rejection_threshold_(rejection_threshold) {}
 
   std::string topic_name_;
   std::vector<bool> update_vector_;
   int update_sum_;
   bool differential_;
   bool relative_;
+  bool pose_use_child_frame_;
   double rejection_threshold_;
 };
 
@@ -147,6 +150,16 @@ class RosFilter : public rclcpp::Node
   void
   controlStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
 
+  //! @brief Differentiate angular velocity for angular acceleration
+  //!
+  //! @param[in] currentTime - The time at which to carry out differentiation (the current time)
+  //!
+  //! 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.
+  //!
+  void differentiateMeasurements(const rclcpp::Time & current_time);
+
   //! @brief Adds a measurement to the queue of measurements to be processed
   //!
   //! @param[in] topic_name - The name of the measurement source (only used for
@@ -263,11 +276,14 @@ class RosFilter : public rclcpp::Node
   //! @param[in] callback_data - Relevant static callback data
   //! @param[in] target_frame - The target frame_id into which to transform the
   //! data
+  //! @param[in] pose_source_frame - The source frame_id from which to transform
+  //! the data
   //! @param[in] imu_data - Whether this data comes from an IMU
   //!
   void poseCallback(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg,
     const CallbackData & callback_data, const std::string & target_frame,
+    const std::string & pose_source_frame,
     const bool imu_data);
 
   //! @brief initialize the filter
@@ -420,6 +436,8 @@ class RosFilter : public rclcpp::Node
   //! @param[in] topic_name - The name of the topic over which this message was
   //! received
   //! @param[in] target_frame - The target tf frame
+  //! @param[in] relative - whether the IMU sensor reports pose relative to
+  //! initialization pose
   //! @param[in] update_vector - The update vector for the data source
   //! @param[in] measurement - The twist data converted to a measurement
   //! @param[in] measurement_covariance - The covariance of the converted
@@ -429,6 +447,7 @@ class RosFilter : public rclcpp::Node
     const sensor_msgs::msg::Imu::SharedPtr msg,
     const std::string & topic_name,
     const std::string & target_frame,
+    const bool relative,
     std::vector<bool> & update_vector,
     Eigen::VectorXd & measurement,
     Eigen::MatrixXd & measurement_covariance);
@@ -438,6 +457,7 @@ class RosFilter : public rclcpp::Node
   //! @param[in] topic_name - The name of the topic over which this message was
   //! received
   //! @param[in] target_frame - The target tf frame
+  //! @param[in] source_frame - The source tf frame
   //! @param[in] differential - Whether we're carrying out differential
   //! integration
   //! @param[in] relative - Whether this measurement is processed relative to
@@ -453,6 +473,7 @@ class RosFilter : public rclcpp::Node
   bool preparePose(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg,
     const std::string & topic_name, const std::string & target_frame,
+    const std::string & source_frame,
     const bool differential, const bool relative, const bool imu_data,
     std::vector<bool> & update_vector, Eigen::VectorXd & measurement,
     Eigen::MatrixXd & measurement_covariance);
@@ -651,6 +672,22 @@ class RosFilter : public rclcpp::Node
   //!
   std::map<std::string, rclcpp::Time> last_message_times_;
 
+  //! @brief Last time mark that time-differentiation is calculated, in seconds
+  //!
+  double last_diff_time_;
+
+  //! @brief Last record of filtered angular velocity
+  //!
+  tf2::Vector3 last_state_twist_rot_;
+
+  //! @brief Calculated angular acceleration from time-differencing
+  //!
+  tf2::Vector3 angular_acceleration_;
+
+  //! @brief Covariance of the calculated angular acceleration
+  //!
+  Eigen::MatrixXd angular_acceleration_cov_;
+
   //! @brief Stores the first measurement from each topic for relative
   //! measurements
   //!

params/ekf.yaml

@@ -116,6 +116,13 @@ ekf_filter_node:
         # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
         odom0_relative: false
 
+        # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry.
+        # Note: this is different from setting odom0_relative to true, as when child_frame is different from 
+        # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame.
+        # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero 
+        # offset from base_link.
+        odom0_pose_use_child_frame: false
+
         # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
         # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
         # numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not