Merge pull request #1 from SamueleSandrini/is_moving

Filter Entity

bt_nodes/bt_test/CMakeLists.txt

@@ -148,6 +148,11 @@ target_link_libraries(carry_my_luggage_test
   hri::dialogConfirmation_bt_node
 )
 
+# FilterEntity test
+add_executable(filter_entity_test src/filter_entity_test.cpp)
+ament_target_dependencies(filter_entity_test ${dependencies})
+target_link_libraries(filter_entity_test perception::filter_entity_bt_node)
+
 install(TARGETS
   set_wp_test
   look_at_test
@@ -167,6 +172,7 @@ install(TARGETS
   follow_person_test
   init_carry
   carry_my_luggage_test
+  filter_entity_test
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

bt_nodes/bt_test/bt_xml/filter_entity_test.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Repeat num_cycles="-1">
+            <Delay delay_msec="50">
+                <Action ID="FilterEntity" frame="person_0" lambda="0.2"/>
+            </Delay>
+        </Repeat>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="FilterEntity">
+            <input_port default="person_0" name="frame">Frame Name</input_port>
+            <input_port default="0.2" name="lambda">Filtering coefficient</input_port>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

bt_nodes/bt_test/src/filter_entity_test.cpp

@@ -0,0 +1,73 @@
+// 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 <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include <tf2_ros/buffer.h>
+
+#include "rclcpp/rclcpp.hpp"
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("filter_entity_test");
+
+  BT::BehaviorTreeFactory factory;
+  BT::SharedLibrary loader;
+
+  factory.registerFromPlugin(loader.getOSName("filter_entity_bt_node"));
+
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_test");
+  std::string xml_file = pkgpath + "/bt_xml/filter_entity_test.xml";
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set("node", node);
+
+  // auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  // auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+  // auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
+
+  // blackboard->set("tf_buffer", tf_buffer);
+  // blackboard->set("tf_broadcaster", tf_broadcaster);
+
+  BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
+
+  // auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);
+
+  rclcpp::Rate rate(10);
+
+  bool finish = false;
+  while (!finish && rclcpp::ok()) {
+    finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
+
+    rclcpp::spin_some(node);
+    rate.sleep();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

bt_nodes/perception/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(tf2 REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(perception_system REQUIRED)
 
+
 set(CMAKE_CXX_STANDARD 17)
 
 set(dependencies
@@ -54,6 +55,12 @@ list(APPEND plugin_libs extract_collision_scene_bt_node)
 add_library(is_entity_moving_bt_node SHARED src/perception/is_entity_moving.cpp)
 list(APPEND plugin_libs is_entity_moving_bt_node)
 
+add_library(is_moving_bt_node SHARED src/perception/is_moving.cpp)
+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(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.hpp

@@ -0,0 +1,77 @@
+// 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_HPP_
+#define PERCEPTION__FILTER_ENTITY_HPP_
+
+#include <string>
+#include <algorithm>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include "tf2_ros/transform_broadcaster.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace perception
+{
+
+using namespace std::chrono_literals;
+
+class FilterEntity : public BT::ActionNodeBase
+{
+public:
+  explicit FilterEntity(
+    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"),
+      });
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  std::string frame_;
+  double lambda_;
+  bool state_obs_initialized_;
+
+  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_HPP_

bt_nodes/perception/include/perception/is_moving.hpp

@@ -0,0 +1,89 @@
+// 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__IS_MOVING_HPP_
+#define PERCEPTION__IS_MOVING_HPP_
+
+#include <string>
+#include <algorithm>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include "tf2_ros/transform_broadcaster.h"
+
+#include "perception_system/PerceptionListener.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
+
+namespace perception
+{
+
+using namespace std::chrono_literals;
+
+class IsMoving : public BT::ConditionNode
+{
+public:
+  explicit IsMoving(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("frame"),
+        BT::InputPort<float>(
+          "velocity_tolerance",
+          "Tolerance in velocity to consider the human stopped"),
+        BT::InputPort<float>("threshold_time", "Lag time to consider the human stopped"),
+        BT::InputPort<float>("position_buffer_dimension", "Human position buffer dimension")
+      });
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  std::string frame_, cam_frame_;
+  float velocity_tolerance_, threshold_time_, position_buffer_dimension_;
+  bool buffer_dim_riched_ = false;
+  bool entity_stopped_ = false;
+  rclcpp::Time first_time_stopped_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::vector<geometry_msgs::msg::TransformStamped> position_buffer_;
+
+  void add_position(const geometry_msgs::msg::TransformStamped & new_position);
+  double compute_velocity();
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_;
+
+};
+
+}  // namespace perception
+
+#endif  // PERCEPTION__IS_MOVING_HPP_

bt_nodes/perception/package.xml

@@ -27,4 +27,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>