Merge branch 'humble-devel' of https://github.com/CoreSenseEU/CoreSense4Home into humble-devel

Author: igonzf

bt_nodes/configuration/CMakeLists.txt

@@ -61,6 +61,9 @@ list(APPEND plugin_libs init_restaurant_bt_node)
 add_library(remove_string_suffix_bt_node SHARED src/configuration/remove_string_suffix.cpp)
 list(APPEND plugin_libs remove_string_suffix_bt_node)
 
+add_library(add_string_suffix_bt_node SHARED src/configuration/add_string_suffix.cpp)
+list(APPEND plugin_libs add_string_suffix_bt_node)
+
 add_library(remove_string_prefix_bt_node SHARED src/configuration/remove_string_prefix.cpp)
 list(APPEND plugin_libs remove_string_prefix_bt_node)
 

bt_nodes/configuration/include/configuration/add_string_suffix.hpp

@@ -0,0 +1,50 @@
+// 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 CONFIGURATION__ADD_STRING_SUFFIX_HPP_
+#define CONFIGURATION__ADD_STRING_SUFFIX_HPP_
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace configuration
+{
+
+class AddStringSuffix : public BT::ActionNodeBase
+{
+public:
+  explicit AddStringSuffix(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>("string_to_modify"),
+        BT::InputPort<std::string>("suffix"),
+        BT::OutputPort<std::string>("result")
+      });
+  }
+
+private:
+  std::string string_to_modify_;
+  std::string suffix_;
+  std::string result_;
+};
+
+}  // namespace configuration
+
+#endif  // CONFIGURATION__ADD_STRING_SUFFIX_HPP_

bt_nodes/configuration/src/configuration/add_string_suffix.cpp

@@ -0,0 +1,45 @@
+// 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 "configuration/add_string_suffix.hpp"
+
+namespace configuration
+{
+
+AddStringSuffix::AddStringSuffix(
+  const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+}
+
+BT::NodeStatus AddStringSuffix::tick()
+{
+  getInput("string_to_modify", string_to_modify_);
+  getInput("suffix", suffix_);
+  if (string_to_modify_.empty()) {
+    return BT::NodeStatus::FAILURE;
+  }
+  result_ = string_to_modify_ + suffix_;
+  setOutput("result", result_);
+  return BT::NodeStatus::SUCCESS;
+}
+
+void AddStringSuffix::halt() {}
+
+}  // namespace configuration
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<configuration::AddStringSuffix>("AddStringSuffix");
+}

bt_nodes/perception/CMakeLists.txt

@@ -112,6 +112,9 @@ list(APPEND plugin_libs clear_octomap_bt_node)
 add_library(switch_yolo_model_bt_node SHARED src/perception/switch_yolo_model.cpp)
 list(APPEND plugin_libs switch_yolo_model_bt_node)
 
+add_library(set_persistent_id_bt_node SHARED src/perception/set_persistent_id.cpp)
+list(APPEND plugin_libs set_persistent_id_bt_node)
+
 add_library(extract_pc_from_class_bt_node SHARED src/perception/extract_pc_from_class.cpp)
 list(APPEND plugin_libs extract_pc_from_class_bt_node)
 
@@ -120,6 +123,8 @@ list(APPEND plugin_libs store_identity_vector_bt_node)
 
 add_library(enable_detect_by_identity_bt_node SHARED src/perception/enable_detect_by_identity.cpp)
 list(APPEND plugin_libs enable_detect_by_identity_bt_node)
+add_library(extract_handover_alignment_bt_node SHARED src/perception/ExtractHandoverAlignment.cpp)
+list(APPEND plugin_libs extract_handover_alignment_bt_node)
 
 foreach(bt_plugin ${plugin_libs})
 ament_target_dependencies(${bt_plugin} ${dependencies})

bt_nodes/perception/include/perception/ExtractHandoverAlignment.hpp

@@ -0,0 +1,71 @@
+// 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__EXTRACT_HANDOVER_ALIGNMENT_HPP_
+#define PERCEPTION__EXTRACT_HANDOVER_ALIGNMENT_HPP_
+
+#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_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "yolo_msgs/msg/detection_array.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+
+namespace perception
+{
+
+class ExtractHandoverAlignment : public BT::ActionNodeBase
+{
+public:
+  explicit ExtractHandoverAlignment(
+    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>("interest_class"),
+       BT::InputPort<double>("target_z_distance"),
+       BT::InputPort<double>("target_y_distance"),
+       BT::OutputPort<double>("base_x_movement"),
+       BT::OutputPort<double>("torso_z_movement")});
+  }
+
+  void detection_callback_(yolo_msgs::msg::DetectionArray::UniquePtr msg);
+  void joint_state_callback_(sensor_msgs::msg::JointState::UniquePtr msg);
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  rclcpp::Subscription<yolo_msgs::msg::DetectionArray>::SharedPtr detected_objs_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  yolo_msgs::msg::DetectionArray::UniquePtr last_detected_objs_ = {nullptr};
+  std::string interest_class_{""};
+  double current_torso_height_ = 0.0;
+  bool torso_height_received_ = false;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+};
+
+}  // namespace perception
+
+#endif  // PERCEPTION__EXTRACT_HANDOVER_ALIGNMENT_HPP_

bt_nodes/perception/include/perception/set_persistent_id.hpp

@@ -0,0 +1,51 @@
+// 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__SET_PERSISTENT_ID_HPP_
+#define PERCEPTION__SET_PERSISTENT_ID_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "perception/bt_service_node.hpp"
+#include "yolo_msgs/srv/set_persitent_id.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace perception
+{
+
+class SetPersistentId : public perception::BtServiceNode<
+    yolo_msgs::srv::SetPersitentID,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit SetPersistentId(
+    const std::string & xml_tag_name, const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  void on_result() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::InputPort<int>("id")});
+  }
+};
+
+}  // namespace perception
+
+#endif  // PERCEPTION__SET_PERSISTENT_ID_HPP_