Added filter entity with Kalman Filter

bt_nodes/perception/CMakeLists.txt

@@ -23,6 +23,8 @@ find_package(OpenCV REQUIRED)
 find_package(perception_system_interfaces REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(state_observers REQUIRED)
+
 # find_package(backward_ros REQUIRED)
 
 
@@ -47,10 +49,12 @@ set(dependencies
   perception_system_interfaces
   sensor_msgs
   std_srvs
+  state_observers
   # backward_ros
 )
 
-include_directories(include)
+include_directories(include
+  ${state_observers_INCLUDE_DIRS}) #TODO(@samu): remove this line
 
 add_library(extract_object_from_scene_bt_node SHARED src/perception/ExtractObjectsFromScene.cpp)
 list(APPEND plugin_libs extract_object_from_scene_bt_node)
@@ -73,6 +77,9 @@ list(APPEND plugin_libs is_moving_bt_node)
 add_library(filter_entity_bt_node SHARED src/perception/filter_entity.cpp)
 list(APPEND plugin_libs filter_entity_bt_node)
 
+add_library(filter_entity_kf_bt_node SHARED src/perception/filter_entity_kf.cpp)
+list(APPEND plugin_libs filter_entity_kf_bt_node)
+
 add_library(is_pointing_bt_node SHARED src/perception/is_pointing.cpp)
 list(APPEND plugin_libs is_pointing_bt_node)
 

bt_nodes/perception/include/perception/filter_entity_kf.hpp

@@ -0,0 +1,78 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PERCEPTION__FILTER_ENTITY_KF_HPP_
+#define PERCEPTION__FILTER_ENTITY_KF_HPP_
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
+#include <string>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "state_observers/kalman_filter.hpp"
+
+namespace perception
+{
+
+using namespace std::chrono_literals;
+
+class FilterEntityKf : public BT::ActionNodeBase
+{
+public:
+  explicit FilterEntityKf(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  void halt();
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("frame"),
+        BT::InputPort<float>("lambda", "filtering parameter"),
+        BT::OutputPort<std::string>("filtered_frame"),
+      });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  std::shared_ptr<state_observer::KalmanFilter> kf_;
+
+  std::string frame_;
+  double lambda_;
+  bool state_obs_initialized_ = false;
+  bool update_dt_ = false
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  geometry_msgs::msg::TransformStamped filtered_entity_;
+
+  geometry_msgs::msg::TransformStamped initialize_state_observer(
+    const geometry_msgs::msg::TransformStamped & entity);
+  geometry_msgs::msg::TransformStamped update_state_observer(
+    const geometry_msgs::msg::TransformStamped & entity);
+};
+
+}  // namespace perception
+
+#endif  // PERCEPTION__FILTER_ENTITY_KF_HPP_

bt_nodes/perception/package.xml

@@ -28,7 +28,8 @@
   <depend>perception_system</depend>
   <depend>perception_system_interfaces</depend>
   <depend>sensor_msgs</depend>
-
+  <depend>state_observers</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

bt_nodes/perception/src/perception/filter_entity_kf.cpp

@@ -0,0 +1,154 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "perception/filter_entity_kf.hpp"
+
+#include <limits>
+#include <string>
+#include <utility>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+namespace perception
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+FilterEntityKf::FilterEntityKf(const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+  config().blackboard->get("node", node_);
+
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  getInput("lambda", lambda_);
+  getInput("update_dT", update_dt_);
+
+  double dT;
+  getInput("dT", dT);
+
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
+  setOutput("filtered_frame", frame_ + "_filtered");
+  RCLCPP_INFO(node_->get_logger(), "FilterEntity initialized");
+
+  // create eigen matrix for A matrix of the kalman filter for a constant velocity model in 3d x,y,z x_dot,y_dot,z_dot
+  Eigen::MatrixXd A(6, 6);
+  A <<  1, 0, 0, dT, 0, 0,  // x
+        0, 1, 0, 0, dT, 0,  // y
+        0, 0, 1, 0, 0, dT,  // z
+        0, 0, 0, 1, 0, 0,  // x_dot
+        0, 0, 0, 0, 1, 0,  // y_dot
+        0, 0, 0, 0, 0, 1;  // z_dot
+  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(6, 1);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(3, 6);
+  C.diagonal().setOnes();
+
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6);
+  Eigen::MatrixXd R = Eigen::MatrixXd::Identity(3, 3);
+
+  kf_ = std::make_shared<state_observer::KalmanFilter>(A, B, C, Q, R);
+  // kf_ = state_observer::KalmanFilter(A,B,C,Q,R);
+}
+
+void FilterEntityKf::halt() {RCLCPP_INFO(node_->get_logger(), "FilterEntity halted");}
+
+geometry_msgs::msg::TransformStamped FilterEntityKf::update_state_observer(
+  const geometry_msgs::msg::TransformStamped & entity)
+{
+  if(update_dt_)
+  {
+    //update A if not fixed dT
+    double dT;
+    //compite dT as the difference between the current time and the last time ros2 time now
+    // update A matrix
+    dT = entity.header.stamp.sec + entity.header.stamp.nanosec * 1e9 - 
+        filtered_entity_.header.stamp.sec - entity.header.stamp.nanosec * 1e9;
+    Eigen::MatrixXd A(6, 6);
+    A <<  1, 0, 0, dT, 0, 0,  // x
+          0, 1, 0, 0, dT, 0,  // y
+          0, 0, 1, 0, 0, dT,  // z
+          0, 0, 0, 1, 0, 0,  // x_dot
+          0, 0, 0, 0, 1, 0,  // y_dot
+          0, 0, 0, 0, 0, 1;  // z_dot
+    kf_->set_state_transition_matrix(A);
+  }
+
+  // measurements 
+  Eigen::VectorXd z(3);
+  z << entity.transform.translation.x, entity.transform.translation.y, entity.transform.translation.z;
+  kf_->update(z);
+  Eigen::VectorXd filtered_output = kf_->get_output();
+
+  filtered_entity_.transform.translation.x =
+    filtered_output(0);
+  filtered_entity_.transform.translation.y =
+    filtered_output(1);
+  filtered_entity_.transform.translation.z =
+    filtered_output(2);
+  filtered_entity_.header.stamp = entity.header.stamp;
+  return filtered_entity_;
+}
+geometry_msgs::msg::TransformStamped FilterEntityKf::initialize_state_observer(
+  const geometry_msgs::msg::TransformStamped & entity)
+{
+  // Create a eigen vector with the x y z of the transform stamped entity 
+  Eigen::VectorXd x0(6);
+  x0 << entity.transform.translation.x, entity.transform.translation.y, entity.transform.translation.z, 0, 0, 0;
+  kf_->initialize(x0);
+  filtered_entity_ = entity;
+  filtered_entity_.child_frame_id = entity.child_frame_id + "_filtered";
+  return filtered_entity_;
+}
+
+BT::NodeStatus FilterEntityKf::tick()
+{
+  getInput("frame", frame_);
+  RCLCPP_INFO(node_->get_logger(), "IsMoving filtering frame %s", frame_.c_str());
+
+  geometry_msgs::msg::TransformStamped entity_transform_now_msg;
+
+  try {
+    entity_transform_now_msg = tf_buffer_->lookupTransform("odom", frame_, tf2::TimePointZero);
+    RCLCPP_INFO(
+      node_->get_logger(), "Position %s to %s: %f %f %f", frame_.c_str(), "odom",
+      entity_transform_now_msg.transform.translation.x,
+      entity_transform_now_msg.transform.translation.y,
+      entity_transform_now_msg.transform.translation.z);
+  } catch (const tf2::TransformException & ex) {
+    RCLCPP_INFO(
+      node_->get_logger(), "Could not transform %s to %s: %s", frame_.c_str(), "odom", ex.what());
+    RCLCPP_INFO(node_->get_logger(), "Cannot transform");
+
+    return BT::NodeStatus::SUCCESS;
+  }
+  geometry_msgs::msg::TransformStamped filtered_entity;
+  if (state_obs_initialized_) {
+    filtered_entity = update_state_observer(entity_transform_now_msg);
+  } else {
+    filtered_entity = initialize_state_observer(entity_transform_now_msg);
+    state_obs_initialized_ = true;
+  }
+  filtered_entity.child_frame_id = frame_ + "_filtered";
+  tf_broadcaster_->sendTransform(filtered_entity);
+  setOutput("filtered_frame", filtered_entity.child_frame_id);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace perception
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<perception::FilterEntityKf>("FilterEntityKf");
+}

robocup_bringup/thirdparty.repos

@@ -59,6 +59,10 @@ repositories:
     type: git
     url: https://github.com/Juancams/navigation_system.git
     version: main
+  ThirdParty/state_observers:
+    type: git
+    url: https://github.com/JRL-CARI-CNR-UNIBS/state_observers.git
+    version: master