Merge pull request #18 from StanfordASL/alvin/estimator_inherit

reorganize estimator inheritence structure

ff_estimate/CMakeLists.txt

@@ -13,7 +13,7 @@ find_package(rclcpp REQUIRED)
 find_package(ff_msgs REQUIRED)
 
 # Estimation library
-add_library(est_lib src/base_estimator.cpp src/pose2d_estimator.cpp)
+add_library(est_lib src/base_mocap_estimator.cpp)
 target_include_directories(est_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -38,10 +38,10 @@ install(
 )
 
 # motion capture estimator node
-add_executable(mocap_estimator_node src/mocap_estimator_node.cpp)
-target_link_libraries(mocap_estimator_node est_lib)
+add_executable(moving_avg_estimator_node src/moving_avg_estimator_node.cpp)
+target_link_libraries(moving_avg_estimator_node est_lib)
 
-install(TARGETS mocap_estimator_node
+install(TARGETS moving_avg_estimator_node
         DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)

ff_estimate/include/ff_estimate/base_estimator.hpp → ff_estimate/include/ff_estimate/base_mocap_estimator.hpp

@@ -29,17 +29,24 @@
 
 #include "ff_msgs/msg/free_flyer_state.hpp"
 #include "ff_msgs/msg/free_flyer_state_stamped.hpp"
+#include "ff_msgs/msg/pose2_d_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace ff
 {
 
 /**
  * @brief estimator base class
  */
-class BaseEstimator : public rclcpp::Node
+class BaseMocapEstimator : public rclcpp::Node
 {
 public:
-  explicit BaseEstimator(const std::string & node_name);
+  /**
+   * @brief constructor
+   *
+   * @param node_name ROS2 node name, default to mocap_estimator
+   */
+  explicit BaseMocapEstimator(const std::string & node_name = "mocap_estimator");
 
 protected:
   /**
@@ -49,8 +56,18 @@ class BaseEstimator : public rclcpp::Node
    */
   void SendStateEstimate(const ff_msgs::msg::FreeFlyerState & state);
 
+  /**
+   * @brief abstract estimator function
+   *
+   * @param pose_stamped time-stamped 2D pose measurement
+   */
+  virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped & pose_stamped) = 0;
+
 private:
   rclcpp::Publisher<ff_msgs::msg::FreeFlyerStateStamped>::SharedPtr state_pub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+
+  void PoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
 };
 
 }  // namespace ff

ff_estimate/src/base_estimator.cpp → ff_estimate/src/base_mocap_estimator.cpp

@@ -21,21 +21,28 @@
 // SOFTWARE.
 
 
-#include "ff_estimate/base_estimator.hpp"
+#include "ff_estimate/base_mocap_estimator.hpp"
 
 using ff_msgs::msg::FreeFlyerState;
 using ff_msgs::msg::FreeFlyerStateStamped;
+using ff_msgs::msg::Pose2DStamped;
+using geometry_msgs::msg::PoseStamped;
 
 namespace ff
 {
 
-BaseEstimator::BaseEstimator(const std::string & node_name)
+BaseMocapEstimator::BaseMocapEstimator(const std::string & node_name)
 : rclcpp::Node(node_name)
 {
+  const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose");
+  pose_sub_ = this->create_subscription<PoseStamped>(
+    pose_channel, rclcpp::SensorDataQoS(), [this](const PoseStamped::SharedPtr msg) {
+      PoseCallback(msg);
+    });
   state_pub_ = this->create_publisher<FreeFlyerStateStamped>("est/state", 10);
 }
 
-void BaseEstimator::SendStateEstimate(const FreeFlyerState & state)
+void BaseMocapEstimator::SendStateEstimate(const FreeFlyerState & state)
 {
   ff_msgs::msg::FreeFlyerStateStamped msg{};
   msg.state = state;
@@ -44,4 +51,19 @@ void BaseEstimator::SendStateEstimate(const FreeFlyerState & state)
   state_pub_->publish(msg);
 }
 
+void BaseMocapEstimator::PoseCallback(const PoseStamped::SharedPtr pose)
+{
+  // convert to 3D pose to 2D pose
+  Pose2DStamped pose2d{};
+
+  pose2d.header = pose->header;
+  pose2d.pose.x = pose->pose.position.x;
+  pose2d.pose.y = pose->pose.position.y;
+  double w = pose->pose.orientation.w;
+  double z = pose->pose.orientation.z;
+  pose2d.pose.theta = std::atan2(2 * w * z, w * w - z * z);
+
+  EstimateWithPose2D(pose2d);
+}
+
 }  // namespace ff

ff_estimate/src/moving_avg_estimator_node.cpp

@@ -0,0 +1,91 @@
+// MIT License
+//
+// Copyright (c) 2023 Stanford Autonomous Systems Lab
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+
+#include "ff_estimate/base_mocap_estimator.hpp"
+
+using ff_msgs::msg::FreeFlyerState;
+using ff_msgs::msg::Pose2DStamped;
+
+class MovingAvgEstimatorNode : public ff::BaseMocapEstimator
+{
+public:
+  MovingAvgEstimatorNode()
+  : ff::BaseMocapEstimator("moving_avg_estimator")
+  {
+    // perform simple 1st order IIR lowpass filter on derivate estimates
+    // coefficient in [0, 1) (higher coefficient filters better with larger delay)
+    this->declare_parameter("lowpass_coeff", .95);
+    this->declare_parameter("min_dt", 0.005);
+  }
+
+  void EstimateWithPose2D(const Pose2DStamped & pose_stamped) override
+  {
+    FreeFlyerState state{};
+
+    state.pose = pose_stamped.pose;
+    if (prev_state_ready_) {
+      const rclcpp::Time now = pose_stamped.header.stamp;
+      const rclcpp::Time last = prev_.header.stamp;
+      double dt = (now - last).seconds();
+
+      // ignore this frame if it is too close to the last frame
+      if (dt < this->get_parameter("min_dt").as_double()) {
+        return;
+      }
+
+      // finite difference
+      double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt;
+      double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt;
+      // wrap angle delta to [-pi, pi]
+      double dtheta = std::remainder(pose_stamped.pose.theta - prev_.state.pose.theta, 2 * M_PI);
+      double wz = dtheta / dt;
+
+      double alpha = this->get_parameter("lowpass_coeff").as_double();
+      if (alpha < 0 || alpha >= 1) {
+        RCLCPP_ERROR(this->get_logger(), "IIR filter disabled: invalid coefficient %f", alpha);
+        alpha = 0;
+      }
+      state.twist.vx = alpha * prev_.state.twist.vx + (1. - alpha) * vx;
+      state.twist.vy = alpha * prev_.state.twist.vy + (1. - alpha) * vy;
+      state.twist.wz = alpha * prev_.state.twist.wz + (1. - alpha) * wz;
+    } else {
+      prev_state_ready_ = true;
+    }
+
+    prev_.state = state;
+    prev_.header = pose_stamped.header;
+
+    SendStateEstimate(state);
+  }
+
+private:
+  ff_msgs::msg::FreeFlyerStateStamped prev_;
+  bool prev_state_ready_ = false;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MovingAvgEstimatorNode>());
+  rclcpp::shutdown();
+}