Ros2 nodes

CMakeLists.txt

@@ -10,14 +10,20 @@ set(ESTIMATION_UTILS_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR})
 
 add_subdirectory(src/kalman)
 
+add_subdirectory(src/force_estimation)
+
 add_subdirectory(src/payload)
 
-#TBD port ROS2
-#add_subdirectory(src/nodes)
+add_subdirectory(src/nodes)
 
 install(DIRECTORY include/${PROJECT_NAME}
         DESTINATION include/${PROJECT_NAME}
         FILES_MATCHING PATTERN *.h)
 
+install(DIRECTORY launch
+        DESTINATION launch)
+install(DIRECTORY config
+        DESTINATION launch)
+
 include(cmake/ExportProject.cmake)
 export_project()

config/doosan_h2515_example.yaml

@@ -0,0 +1,25 @@
+force_estimator:
+  rate: 100.0
+  use_momentum_based: false
+  is_model_floating_base: false
+  model_type: pyn
+  links: [link_6]
+  chains: [arm]
+  link_6:
+    dofs: [0, 1, 2, 3, 4, 5]
+  ignored_joints: [dagana_1_claw_joint]
+  svd_threshold: 0.05
+  #reset offset
+  sec_to_reset: 3
+  #log
+  enable_log: false
+  #ref frame wrt forces are published
+  ref_frame: base_link
+  #filter
+  enable_filter: false
+  filter_damp: 0.8
+  filter_bw: 3
+  filter_dead_zone: 0
+  #markers
+  pub_arrows: false
+

include/estimation_utils/payload/force_estimation.h → include/estimation_utils/force_estimation/force_estimation.h

@@ -1,11 +1,13 @@
 #ifndef ESTIMATION_UTILS_FORCE_EST_H
 #define ESTIMATION_UTILS_FORCE_EST_H
 
+#include <set>
 #include <xbot2_interface/xbotinterface2.h>
 #include <algorithm>
+// #include <cartesian_interface/Macro.h>
 #include <matlogger2/matlogger2.h>
 
-#include <set>
+#include <estimation_utils/utils/second_order_filter.h>
 
 namespace estimation_utils
 {
@@ -15,10 +17,9 @@ class ForceEstimation
 
 public:
 
-    typedef std::shared_ptr<ForceEstimation> Ptr;
-    typedef std::weak_ptr<ForceEstimation> WeakPtr;
+    // CARTESIO_DECLARE_SMART_PTR(ForceEstimation)
 
-    static const double DEFAULT_SVD_THRESHOLD;
+    static constexpr double DEFAULT_SVD_THRESHOLD = 0.05;
 
     /**
      * @brief ForceEstimation constructor.
@@ -27,6 +28,7 @@ class ForceEstimation
      * @param svd_threshold: threshold for solution regularization (close to singularities)
      */
     ForceEstimation(XBot::ModelInterface::ConstPtr model,
+                    double rate,
                     double svd_threshold = DEFAULT_SVD_THRESHOLD);
 
     /**
@@ -58,9 +60,14 @@ class ForceEstimation
 
     bool getResiduals(Eigen::VectorXd &res) const;
 
+    void resetOffset(double sec=3);
+
+    bool initFilter(const double& damping = 0.8, const double& bw = 3, const double& dead_zone = 0);
+
 protected:
 
     XBot::ModelInterface::ConstPtr _model;
+    double _rate;
 
 private:
 
@@ -72,7 +79,9 @@ class ForceEstimation
         XBot::ForceTorqueSensor::Ptr sensor;
         std::vector<int> dofs;
         std::string link_name;
-
+        //probably is better to store this into the forceTorque class
+        Eigen::Vector6d wrench_offset;
+        estimation_utils::utils::FilterWrap<Eigen::Vector6d>::Ptr filter;
     };
 
     std::set<int> _ignore_idx;
@@ -90,6 +99,18 @@ class ForceEstimation
     Eigen::JacobiSVD<Eigen::MatrixXd> _svd;
     Eigen::ColPivHouseholderQR<Eigen::MatrixXd> _qr;
 
+    //reset the offset things
+    bool _reset_offset_requested = false;
+    bool _reset_offset_running  = false;
+    unsigned int _reset_offset_i = 0;
+    double _reset_offset_N = 0;
+
+    //filter things. For now all ft have the same filter params
+    double _filter_damping;
+    double _filter_bw;
+    double _filter_dead_zone;
+    bool _filter_in_use = false;
+
 };
 
 class ForceEstimationMomentumBased : public ForceEstimation
@@ -100,10 +121,9 @@ class ForceEstimationMomentumBased : public ForceEstimation
     // CARTESIO_DECLARE_SMART_PTR(ForceEstimationMomentumBased);
 
     static constexpr double DEFAULT_OBS_BW = 4.0;
-    static constexpr double DEFAULT_RATE = 200.0;
 
     ForceEstimationMomentumBased(XBot::ModelInterface::ConstPtr model,
-                                 double rate = DEFAULT_RATE,
+                                 double rate,
                                  double svd_threshold = DEFAULT_SVD_THRESHOLD,
                                  double obs_bw = DEFAULT_OBS_BW);
 
@@ -114,7 +134,7 @@ class ForceEstimationMomentumBased : public ForceEstimation
 
     void init_momentum_obs();
 
-    double _rate, _k_obs;
+    double _k_obs;
 
     Eigen::VectorXd _y, _tau, _g, _b, _sol;
     Eigen::VectorXd _p0, _p1, _p2, _q, _qdot, _q_old, _h, _coriolis, _y_static;

include/estimation_utils/nodes/force_estimation_node.h

@@ -0,0 +1,69 @@
+#ifndef ESTIMATION_UTILS_FORCE_ESTIMATION_NODE_H
+#define ESTIMATION_UTILS_FORCE_ESTIMATION_NODE_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <xbot2_interface/robotinterface2.h>
+#include <xbot2_interface/ros2/config_from_param.hpp>
+
+#include <matlogger2/matlogger2.h>
+//for waitForXbotCore method
+#include <xbot_msgs/srv/plugin_status.hpp>
+
+#include <estimation_utils/force_estimation/force_estimation.h>
+
+namespace estimation_utils {
+
+class ForceEstimationNode : public rclcpp::Node {
+
+public:
+    ForceEstimationNode();
+
+private:              
+
+    rclcpp::TimerBase::SharedPtr _timer;
+
+    double _rate;
+
+    XBot::RobotInterface::Ptr _robot;
+    XBot::ModelInterface::Ptr _model;
+    XBot::ImuSensor::ConstPtr _imu;
+
+    std::shared_ptr<estimation_utils::ForceEstimation> _f_est_ptr;
+    Eigen::VectorXd _tau, _tau_offset;
+    geometry_msgs::msg::WrenchStamped _wrench_msg;
+
+    std::map<std::string, XBot::ForceTorqueSensor::ConstPtr> _ft_map;
+    std::map<std::string, rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr> _ft_pub_map;
+
+    void publishArrows();  
+    bool run();
+
+    //offset reset
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr _request_wrench_zero_offset;
+    double _reset_time_sec;
+    bool _reset_requested = false;
+    bool wrenchZeroOffsetClbk(const std::shared_ptr<std_srvs::srv::Empty::Request> req, 
+                              std::shared_ptr<std_srvs::srv::Empty::Response> res);
+
+    //markers pub
+    bool _pub_markers;
+    double _arrow_scale_factor, _arrow_max_norm;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr _arrows_pub;
+    visualization_msgs::msg::Marker _marker;
+
+    std::string _ref_frame;
+
+    //log
+    XBot::MatLogger2::Ptr _logger;
+
+    bool waitForXbotCore(double timeout_sec = -1);
+};
+
+} //namespace
+
+#endif // ESTIMATION_UTILS_FORCE_ESTIMATION_NODE_H
+

include/estimation_utils/payload/payload_estimation.h

@@ -3,7 +3,7 @@
 
 #include <Eigen/Dense>
 #include <xbot2_interface/xbotinterface2.h>
-#include <estimation_utils/payload/force_estimation.h>
+#include <estimation_utils/force_estimation/force_estimation.h>
 #include "../kalman/kalman.h"
 
 namespace estimation_utils