hri nodes enhanced

.gitignore

@@ -0,0 +1,5 @@
+.gitingore
+_*.hpp
+_*.cpp
+_*.txt
+_*.xml

attention/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.8)
+project(attention_bt_nodes)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_cascade_lifecycle REQUIRED)
+find_package(attention_system REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+
+set(CMAKE_CXX_STANDARD 17)
+
+set(dependencies
+  ament_index_cpp
+  rclcpp
+  rclcpp_cascade_lifecycle
+  attention_system
+  behaviortree_cpp_v3
+)
+
+include_directories(include)
+
+set(plugin_sources
+  src/attention/activate_attention.cpp
+  src/attention/deactivate_attention.cpp
+)
+
+set(plugin_libs "")
+
+foreach(src ${plugin_sources})
+  get_filename_component(plugin ${src} NAME_WE)
+  add_library(${plugin}_bt_node SHARED ${src})
+  ament_target_dependencies(${plugin}_bt_node ${dependencies})
+  target_compile_definitions(${plugin}_bt_node PRIVATE BT_PLUGIN_EXPORT)
+  list(APPEND plugin_libs ${plugin}_bt_node)
+endforeach()
+
+add_executable(attention_test src/main_attention.cpp)
+ament_target_dependencies(attention_test ${dependencies})
+
+install(TARGETS
+  attention_test
+  ${plugin_libs}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include launch config bt_xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

attention/README.md

@@ -0,0 +1,2 @@
+# perception_bt_nodes
+A collection of BT nodes for perception capabilities

attention/bt_xml/attend.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <Action ID="ActivateAttention" frame_id="person_1" what="base"/>
+            <Timeout msec="10000">
+                <RetryUntilSuccessful num_attempts="-1">
+                    <AlwaysFailure/>
+                </RetryUntilSuccessful>
+            </Timeout>
+            <Action ID="DeactivateAttention" what="base"/>
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="ActivateAttention">
+            <input_port name="frame_id"/>
+            <input_port default="base" name="what"/>
+        </Action>
+        <Action ID="DeactivateAttention">
+            <input_port default="base" name="what"/>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

attention/config/attention.yaml

@@ -0,0 +1,7 @@
+attention_node:
+  ros__parameters:
+    use_sim_time: True
+    bt_xml_file: attend.xml
+    plugins:
+    - activate_attention_bt_node
+    - deactivate_attention_bt_node

attention/include/attention/activate_attention.hpp

@@ -0,0 +1,60 @@
+// Copyright 2024 Intelligent Robotics Lab
+//
+// 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 ATTENTION_ACTIVATE__HPP_
+#define ATTENTION_ACTIVATE__HPP_
+
+#include <memory>
+#include <string>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "std_msgs/msg/string.hpp"
+
+
+namespace attention
+{
+
+class ActivateAttention : public BT::ActionNodeBase
+{
+public:
+  explicit ActivateAttention(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  BT::NodeStatus tick();
+  void halt();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("what"),
+        BT::InputPort<std::string>("frame_id"),
+      });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+
+  std::string frame_id_, what_;
+
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr attention_pub_;
+
+};
+
+}  // namespace attention
+
+#endif  // ATTENTION_ACTIVATE__HPP_

attention/include/attention/deactivate_attention.hpp

@@ -0,0 +1,58 @@
+// Copyright 2024 Intelligent Robotics Lab
+//
+// 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 ATTENTION_DEACTIVATE__HPP_
+#define ATTENTION_DEACTIVATE__HPP_
+
+#include <memory>
+#include <string>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "std_msgs/msg/string.hpp"
+
+namespace attention
+{
+
+class DeactivateAttention : public BT::ActionNodeBase
+{
+public:
+  explicit DeactivateAttention(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  BT::NodeStatus tick();
+  void halt();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("what")
+      });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+
+  std::string what_;
+
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr attention_pub_;
+
+};
+
+}  // namespace attention
+
+#endif  // ATTENTION_DEACTIVATE__HPP_

attention/launch/attention.launch.py

@@ -0,0 +1,48 @@
+# Copyright 2023 Rodrigo Pérez-Rodríguez
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+import yaml
+
+def generate_launch_description():
+    # Get the launch directory
+    pkg_dir = get_package_share_directory('attention_bt_nodes')
+
+    params_file = os.path.join(pkg_dir, 'config', 'attention.yaml')
+    with open(params_file, 'r') as f:
+        params = yaml.safe_load(f)['attention_node']['ros__parameters']
+
+    ld = LaunchDescription()
+
+    nav_cmd = Node(
+        package='attention_bt_nodes',
+        executable='attention_test',
+        output='screen',
+        remappings=[
+        ],
+        parameters=[{
+            'use_sim_time': True,
+        }, params]
+    )
+
+    ld.add_action(nav_cmd)
+
+    return ld
+

attention/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>attention_bt_nodes</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="rodrigo.perez@urjc.es">rod</maintainer>
+
+  <license>Apache License, Version 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_cascade_lifecycle</depend>
+  <depend>behaviortree_cpp_v3</depend>
+
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

attention/src/attention/activate_attention.cpp

@@ -0,0 +1,62 @@
+// Copyright 2024 Intelligent Robotics Lab
+//
+// 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 "attention/activate_attention.hpp"
+
+namespace attention
+{
+
+
+ActivateAttention::ActivateAttention(const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+  std::string what;
+
+  config().blackboard->get("node", node_);
+
+  getInput("frame_id", frame_id_);
+  getInput("what", what);
+  if (what == "base") {
+    node_->add_activation("attention_system_base");
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Unknown what: %s. Activating generic", what.c_str());
+    node_->add_activation("attention_system_base");
+  }
+
+  attention_pub_ = node_->create_publisher<std_msgs::msg::String>("attention_system", 10);
+  attention_pub_->on_activate();
+}
+BT::NodeStatus ActivateAttention::tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "ACTIVATE_ATTENTION");
+
+  std_msgs::msg::String msg;
+  msg.data = frame_id_;
+  attention_pub_->publish(msg);
+
+  rclcpp::spin_some(node_->get_node_base_interface());
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void ActivateAttention::halt()
+{
+  attention_pub_->on_deactivate();
+}
+
+}  // namespace attention
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<attention::ActivateAttention>("ActivateAttention");
+}