diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6063d18 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.gitingore +_*.hpp +_*.cpp +_*.txt +_*.xml \ No newline at end of file diff --git a/attention/CMakeLists.txt b/attention/CMakeLists.txt new file mode 100644 index 0000000..16dba73 --- /dev/null +++ b/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() diff --git a/attention/README.md b/attention/README.md new file mode 100644 index 0000000..c5a8c58 --- /dev/null +++ b/attention/README.md @@ -0,0 +1,2 @@ +# perception_bt_nodes +A collection of BT nodes for perception capabilities diff --git a/attention/bt_xml/attend.xml b/attention/bt_xml/attend.xml new file mode 100644 index 0000000..e21426e --- /dev/null +++ b/attention/bt_xml/attend.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/attention/config/attention.yaml b/attention/config/attention.yaml new file mode 100644 index 0000000..fdd656e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/attention/include/attention/activate_attention.hpp b/attention/include/attention/activate_attention.hpp new file mode 100644 index 0000000..055f36c --- /dev/null +++ b/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 +#include +#include + +#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("what"), + BT::InputPort("frame_id"), + }); + } + +private: + std::shared_ptr node_; + + std::string frame_id_, what_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr attention_pub_; + +}; + +} // namespace attention + +#endif // ATTENTION_ACTIVATE__HPP_ diff --git a/attention/include/attention/deactivate_attention.hpp b/attention/include/attention/deactivate_attention.hpp new file mode 100644 index 0000000..d8c9265 --- /dev/null +++ b/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 +#include +#include + +#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("what") + }); + } + +private: + std::shared_ptr node_; + + std::string what_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr attention_pub_; + +}; + +} // namespace attention + +#endif // ATTENTION_DEACTIVATE__HPP_ diff --git a/attention/launch/attention.launch.py b/attention/launch/attention.launch.py new file mode 100644 index 0000000..aa61df7 --- /dev/null +++ b/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 + diff --git a/attention/package.xml b/attention/package.xml new file mode 100644 index 0000000..73a8917 --- /dev/null +++ b/attention/package.xml @@ -0,0 +1,25 @@ + + + + attention_bt_nodes + 0.0.0 + TODO: Package description + rod + + Apache License, Version 2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclcpp_cascade_lifecycle + behaviortree_cpp_v3 + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/attention/src/attention/activate_attention.cpp b/attention/src/attention/activate_attention.cpp new file mode 100644 index 0000000..ede6aca --- /dev/null +++ b/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("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("ActivateAttention"); +} diff --git a/attention/src/attention/deactivate_attention.cpp b/attention/src/attention/deactivate_attention.cpp new file mode 100644 index 0000000..4fdd2f9 --- /dev/null +++ b/attention/src/attention/deactivate_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/deactivate_attention.hpp" + +namespace attention +{ + + +DeactivateAttention::DeactivateAttention(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("what", what); + if (what == "base") { + node_->remove_activation("attention_system_base"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Unknown what: %s. Deactivating generic", what.c_str()); + node_->remove_activation("attention_system_base"); + } + + attention_pub_ = node_->create_publisher("attention_system", 10); + attention_pub_->on_activate(); +} +BT::NodeStatus DeactivateAttention::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "DEACTIVATE_ATTENTION"); + + std_msgs::msg::String msg; + msg.data = ""; + attention_pub_->publish(msg); + + rclcpp::spin_some(node_->get_node_base_interface()); + + return BT::NodeStatus::SUCCESS; +} + +void DeactivateAttention::halt() +{ + attention_pub_->on_deactivate(); +} + +} // namespace attention + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("DeactivateAttention"); +} + diff --git a/attention/src/main_attention.cpp b/attention/src/main_attention.cpp new file mode 100644 index 0000000..5f9a24f --- /dev/null +++ b/attention/src/main_attention.cpp @@ -0,0 +1,75 @@ +// 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 +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("attention_node"); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + std::vector plugins; + std::string bt_xml_file; + node->declare_parameter("plugins", plugins); + node->declare_parameter("bt_xml_file", bt_xml_file); + node->get_parameter("plugins", plugins); + node->get_parameter("bt_xml_file", bt_xml_file); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + for (const auto & plugin : plugins) { + RCLCPP_INFO(node->get_logger(), "Loading BT Node: [%s]", plugin.c_str()); + factory.registerFromPlugin(loader.getOSName(plugin)); + } + + std::string pkgpath = ament_index_cpp::get_package_share_directory("attention_bt_nodes"); + std::string xml_file = pkgpath + "/bt_xml/" + bt_xml_file; + + RCLCPP_INFO(node->get_logger(), "Loading BT: [%s]", xml_file.c_str()); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", node); + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + + RCLCPP_INFO(node->get_logger(), "Starting behavior from BT"); + + auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); + + rclcpp::Rate rate(15); + + bool finish = false; + while (!finish && rclcpp::ok()) { + finish = tree.tickRoot() != BT::NodeStatus::RUNNING; + rclcpp::spin_some(node->get_node_base_interface()); + rate.sleep(); + } + + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + + rclcpp::shutdown(); + return 0; +} diff --git a/attention/thirdparty.repos b/attention/thirdparty.repos new file mode 100644 index 0000000..b32b169 --- /dev/null +++ b/attention/thirdparty.repos @@ -0,0 +1,9 @@ +repositories: + thirdparty/perception_system: + type: git + url: https://github.com/jmguerreroh/perception_system + version: main + thirdparty/cascade_lifecycle: + type: git + url: https://github.com/fmrico/cascade_lifecycle + version: main diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt new file mode 100644 index 0000000..e1b24db --- /dev/null +++ b/hri/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.8) +project(hri_bt_nodes) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_cascade_lifecycle REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(behaviortree_cpp_v3 REQUIRED) +find_package(audio_common_msgs REQUIRED) +find_package(whisper_msgs REQUIRED) +find_package(llama_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(audio_common_msgs REQUIRED) + +find_package(ZMQ) +if(ZMQ_FOUND) + message(STATUS "ZeroMQ found.") + add_definitions(-DZMQ_FOUND) +else() + message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.") +endif() + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + rclcpp + ament_index_cpp + rclcpp_cascade_lifecycle + rclcpp_action + behaviortree_cpp_v3 + audio_common_msgs + whisper_msgs + llama_msgs + std_msgs + sensor_msgs + std_srvs + audio_common_msgs +) + +include_directories(include ${ZMQ_INCLUDE_DIRS}) + +add_library(listen_bt_node SHARED src/hri/dialog/listen.cpp) +add_library(speak_bt_node SHARED src/hri/dialog/speak.cpp) +add_library(dialog_confirmation_bt_node SHARED src/hri/dialog/dialog_confirmation.cpp) +add_library(query_bt_node SHARED src/hri/dialog/query.cpp) + +list(APPEND plugin_libs + listen_bt_node + speak_bt_node + dialog_confirmation_bt_node + query_bt_node +) + +foreach(bt_plugin ${plugin_libs}) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +endforeach() + +add_executable(hri_test src/main_hri.cpp) +ament_target_dependencies(hri_test ${dependencies}) +target_link_libraries(hri_test ${ZMQ_LIBRARIES}) + +install(TARGETS + hri_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) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) + +foreach(bt_plugin ${plugin_libs}) + ament_export_libraries(${bt_plugin}) +endforeach() + +ament_package() diff --git a/hri/README.md b/hri/README.md new file mode 100644 index 0000000..6f5c3a5 --- /dev/null +++ b/hri/README.md @@ -0,0 +1,2 @@ +# hri_bt_nodes +A collection of BT nodes for HRI capabilities diff --git a/hri/bt_xml/confirm.xml b/hri/bt_xml/confirm.xml new file mode 100644 index 0000000..d54aed3 --- /dev/null +++ b/hri/bt_xml/confirm.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + What was heard + + + + + + + + diff --git a/hri/bt_xml/listen_and_repeat.xml b/hri/bt_xml/listen_and_repeat.xml new file mode 100644 index 0000000..7e2ae50 --- /dev/null +++ b/hri/bt_xml/listen_and_repeat.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + What was heard + + + + + + + + diff --git a/hri/bt_xml/query.xml b/hri/bt_xml/query.xml new file mode 100644 index 0000000..0209d18 --- /dev/null +++ b/hri/bt_xml/query.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + What was heard + + + + + + + + + + + + + diff --git a/hri/bt_xml/speak.xml b/hri/bt_xml/speak.xml new file mode 100644 index 0000000..4e0aa88 --- /dev/null +++ b/hri/bt_xml/speak.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/hri/config/grammar.txt b/hri/config/grammar.txt new file mode 100644 index 0000000..f83102b --- /dev/null +++ b/hri/config/grammar.txt @@ -0,0 +1,25 @@ +R"(root ::= object +value ::= object | array | string | number | ("true" | "false" | "null") ws + +object ::= + "{" ws ( + string ":" ws value + ("," ws string ":" ws value)* + )? "}" ws + +array ::= + "[" ws ( + value + ("," ws value)* + )? "]" ws + +string ::= + "\"" ( + [^"\\] | + "\\" (["\\/bfnrt] | "u" [0-9a-fA-F] [0-9a-fA-F] [0-9a-fA-F] [0-9a-fA-F]) # escapes + )* "\"" ws + +number ::= ("-"? ([0-9] | [1-9] [0-9]*)) ("." [0-9]+)? ([eE] [-+]? [0-9]+)? ws + +# Optional space: by convention, applied in this grammar after literal chars when allowed +ws ::= ([ \t\n] ws)?)" diff --git a/hri/config/hri.yaml b/hri/config/hri.yaml new file mode 100644 index 0000000..79f3cd5 --- /dev/null +++ b/hri/config/hri.yaml @@ -0,0 +1,13 @@ +hri_node: + ros__parameters: + use_sim_time: False + bt_xml_file: confirm.xml + prompt_file: prompt.txt + grammar_file: grammar.txt + placeholder: "[]" + plugins: + - speak_bt_node + - listen_bt_node + - query_bt_node + - dialog_confirmation_bt_node + diff --git a/hri/config/prompt.txt b/hri/config/prompt.txt new file mode 100644 index 0000000..1657f8b --- /dev/null +++ b/hri/config/prompt.txt @@ -0,0 +1,4 @@ +Given the sentence "[]", extract the [] from the sentence and return it with the following JSON format: +{ +"intention": "word extracted in the sentence" +} diff --git a/hri/include/ctrl_support/BTActionNode.hpp b/hri/include/ctrl_support/BTActionNode.hpp new file mode 100644 index 0000000..44cdb56 --- /dev/null +++ b/hri/include/ctrl_support/BTActionNode.hpp @@ -0,0 +1,275 @@ +// Copyright (c) 2018 Intel Corporation +// +// 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 CTRL_SUPPORT__BTACTIONNODE_HPP_ +#define CTRL_SUPPORT__BTACTIONNODE_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/action_node.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace hri +{ + +using namespace std::chrono_literals; // NOLINT + +template +class BtActionNode : public BT::ActionNodeBase +{ +public: + BtActionNode( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) + : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) + { + node_ = config().blackboard->get("node"); + + server_timeout_ = 1s; + + // Initialize the input and output messages + goal_ = typename ActionT::Goal(); + result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); + + std::string remapped_action_name; + if (getInput("server_name", remapped_action_name)) { + action_name_ = remapped_action_name; + } + createActionClient(action_name_); + + // Give the derive class a chance to do any initialization + RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); + } + + BtActionNode() = delete; + + virtual ~BtActionNode() {} + + // Create instance of an action server + void createActionClient(const std::string & action_name) + { + // Now that we have the ROS node to use, create the action client for this BT action + action_client_ = rclcpp_action::create_client(node_, action_name); + + // Make sure the server is actually there before continuing + RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + action_client_->wait_for_action_server(); + } + + // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method + // and call providedBasicPorts in it. + static BT::PortsList providedBasicPorts(BT::PortsList addition) + { + BT::PortsList basic = { + BT::InputPort("server_name", "Action server name"), + BT::InputPort("server_timeout")}; + basic.insert(addition.begin(), addition.end()); + + return basic; + } + + static BT::PortsList providedPorts() {return providedBasicPorts({});} + + // Derived classes can override any of the following methods to hook into the + // processing for the action: on_tick, on_wait_for_result, and on_success + + // Could do dynamic checks, such as getting updates to values on the blackboard + virtual void on_tick() {} + + // There can be many loop iterations per tick. Any opportunity to do something after + // a timeout waiting for a result that hasn't been received yet + virtual void on_wait_for_result() {} + + // Called upon successful completion of the action. A derived class can override this + // method to put a value on the blackboard, for example. + virtual BT::NodeStatus on_success() {return BT::NodeStatus::SUCCESS;} + + // Called when a the action is aborted. By default, the node will return FAILURE. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_aborted() {return BT::NodeStatus::FAILURE;} + + // Called when a the action is cancelled. By default, the node will return SUCCESS. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_cancelled() {return BT::NodeStatus::SUCCESS;} + + // The main override required by a BT action + BT::NodeStatus tick() override + { + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) { + createActionClient(action_name_); + + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + + // user defined callback + try { + on_tick(); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + return BT::NodeStatus::FAILURE; + } + + on_new_goal_received(); + } + + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" + on_wait_for_result(); + + auto goal_status = goal_handle_->get_status(); + if ( + goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + on_new_goal_received(); + } + + rclcpp::spin_some(node_->get_node_base_interface()); + + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } + } + + switch (result_.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + return on_success(); + + case rclcpp_action::ResultCode::ABORTED: + return on_aborted(); + + case rclcpp_action::ResultCode::CANCELED: + return on_cancelled(); + + default: + throw std::logic_error("BtActionNode::Tick: invalid status value"); + } + } + + // The other (optional) override required by a BT action. In this case, we + // make sure to cancel the ROS2 action if it is still running. + void halt() override + { + if (should_cancel_goal()) { + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + if ( + rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); + } + } + + setStatus(BT::NodeStatus::IDLE); + } + +protected: + bool should_cancel_goal() + { + // Shut the node down if it is currently running + if (status() != BT::NodeStatus::RUNNING) { + return false; + } + + rclcpp::spin_some(node_->get_node_base_interface()); + auto status = goal_handle_->get_status(); + + // Check if the goal is still executing + return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; + } + + void on_new_goal_received() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + // TODO(#1652): a work around until rcl_action interface is updated + // if goal ids are not matched, the older goal call this callback so ignore the result + // if matched, it must be processed (including aborted) + if (this->goal_handle_->get_goal_id() == result.goal_id) { + goal_result_available_ = true; + result_ = result; + } + }; + + send_goal_options.feedback_callback = std::bind( + &BtActionNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + + if ( + rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("send_goal failed"); + } + + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + } + + void feedback_callback( + typename rclcpp_action::ClientGoalHandle::SharedPtr, + const typename ActionT::Feedback::ConstSharedPtr feedback) + { + // config().blackboard->set("feedback", feedback); + feedback_ = *feedback; + on_feedback(); + } + + virtual void on_feedback() {} + + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + + std::string action_name_; + typename std::shared_ptr> action_client_; + + // All ROS2 actions have a goal and a result + typename ActionT::Goal goal_; + typename ActionT::Feedback feedback_; + bool goal_updated_{false}; + bool goal_result_available_{false}; + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + typename rclcpp_action::ClientGoalHandle::WrappedResult result_; + // The node that will be used for any ROS operations + typename NodeT::SharedPtr node_; + + // The timeout value while waiting for response from a server when a + // new action goal is sent or canceled + std::chrono::milliseconds server_timeout_; +}; + +} // namespace hri + +#endif // CTRL_SUPPORT__BTACTIONNODE_HPP_ diff --git a/hri/include/hri/dialog/dialog_confirmation.hpp b/hri/include/hri/dialog/dialog_confirmation.hpp new file mode 100644 index 0000000..91ab0cc --- /dev/null +++ b/hri/include/hri/dialog/dialog_confirmation.hpp @@ -0,0 +1,65 @@ +// 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 andGO2OBJECT +// limitations under the License. + +#ifndef HRI__DIALOGCONFIRMATION_HPP_ +#define HRI__DIALOGCONFIRMATION_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "std_msgs/msg/int8.hpp" +#include "whisper_msgs/action/stt.hpp" + +#include "std_msgs/msg/int8.hpp" + +namespace hri +{ + +class DialogConfirmation + : public hri::BtActionNode< + whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit DialogConfirmation( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("language"), // es/en + }); + } + +private: + rclcpp::Publisher::SharedPtr speech_start_publisher_; + + std::string lang_; + + const int START_LISTENING_{0}; +}; + +} // namespace dialog + +#endif // HRI__DIALOGCONFIRMATION_HPP_ diff --git a/hri/include/hri/dialog/listen.hpp b/hri/include/hri/dialog/listen.hpp new file mode 100644 index 0000000..572fdbe --- /dev/null +++ b/hri/include/hri/dialog/listen.hpp @@ -0,0 +1,58 @@ +// 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 andGO2OBJECT +// limitations under the License. + +#ifndef HRI__LISTEN_HPP_ +#define HRI__LISTEN_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "std_msgs/msg/int8.hpp" +#include "whisper_msgs/action/stt.hpp" + + +namespace dialog +{ + +class Listen : public hri::BtActionNode< + whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit Listen( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList({BT::OutputPort("listened_text")}); + } + +private: + rclcpp::Publisher::SharedPtr listen_start_publisher_; // To indicate the robot is listening + + const int START_LISTENING_{0}; +}; + +} // namespace dialog + +#endif // HRI__LISTEN_HPP_ diff --git a/hri/include/hri/dialog/query.hpp b/hri/include/hri/dialog/query.hpp new file mode 100644 index 0000000..0ba5a76 --- /dev/null +++ b/hri/include/hri/dialog/query.hpp @@ -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 andGO2OBJECT +// limitations under the License. + +#ifndef HRI__QUERY_HPP_ +#define HRI__QUERY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "llama_msgs/action/generate_response.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "std_msgs/msg/int8.hpp" + +namespace dialog +{ + +class Query +: public hri::BtActionNode< + llama_msgs::action::GenerateResponse, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit Query( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("text"), + BT::InputPort("intention"), + BT::OutputPort("intention_value") + }); + } + +private: + std::string load_text_file(std::string path); + std::string swap_placeholders(std::string text, std::vector elems); + + std::string prompt_file_path_; + std::string grammar_file_path_; + std::string placeholder_{"[]"}; + + const int START_SPEECH_{1}; +}; + +} // namespace dialog + +#endif // HRI__QUERY_HPP_ diff --git a/hri/include/hri/dialog/speak.hpp b/hri/include/hri/dialog/speak.hpp new file mode 100644 index 0000000..b8c5728 --- /dev/null +++ b/hri/include/hri/dialog/speak.hpp @@ -0,0 +1,72 @@ +// 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 andGO2OBJECT +// limitations under the License. + +#ifndef DIALOG__Speak_HPP_ +#define DIALOG__Speak_HPP_ + +#include +#include +#include +#include +#include + +#include "audio_common_msgs/action/tts.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_msgs/msg/string.hpp" + +namespace dialog +{ + +class Speak : public hri::BtActionNode< + audio_common_msgs::action::TTS, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit Speak( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("speech_text"), + BT::InputPort("params") + }); + } + +private: + BT::NodeStatus on_idle(); + std::string swap_placeholders(std::string text, std::vector elems); + + + std::shared_ptr node_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr speech_text_publisher_; // To publish the text to be spoken + rclcpp_lifecycle::LifecyclePublisher::SharedPtr speech_start_publisher_; // To indicate the start of the speech + + std::string placeholder_{"[]"}; + + const int START_SPEECH_{1}; +}; + +} // namespace dialog + +#endif // HRI__Speak_HPP_ diff --git a/hri/launch/hri.launch.py b/hri/launch/hri.launch.py new file mode 100644 index 0000000..330f661 --- /dev/null +++ b/hri/launch/hri.launch.py @@ -0,0 +1,50 @@ +# 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('hri_bt_nodes') + + params_file = os.path.join(pkg_dir, 'config', 'hri.yaml') + # print('params_file: ', params_file) + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['hri_node']['ros__parameters'] + # print(params) + + ld = LaunchDescription() + + hri_cmd = Node( + package='hri_bt_nodes', + executable='hri_test', + output='screen', + remappings=[ + ], + parameters=[{ + 'use_sim_time': True, + }, params] + ) + + ld.add_action(hri_cmd) + + return ld + diff --git a/hri/launch/hri_dependencies.launch.py b/hri/launch/hri_dependencies.launch.py new file mode 100644 index 0000000..9c8bb34 --- /dev/null +++ b/hri/launch/hri_dependencies.launch.py @@ -0,0 +1,92 @@ +# 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 +from llama_bringup.utils import create_llama_launch +import yaml + +def generate_launch_description(): + + whisper = True + llama = True + + # Get the launch directories for other packages + whisper_dir = get_package_share_directory('whisper_bringup') + + llama_cmd = create_llama_launch( + n_ctx=2048, + n_batch=256, + n_gpu_layers=23, + n_threads=4, + n_predict=-1, + + model_repo='TheBloke/Marcoroni-7B-v3-GGUF', + model_filename='marcoroni-7b-v3.Q3_K_L.gguf', + + prefix='\n\n### Instruction:\n', + suffix='\n\n### Response:\n', + stopping_words=["\n\n\n\n"], + ) + + whisper_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(whisper_dir, 'launch', 'whisper.launch.py') + ), + launch_arguments={'stream': 'False', + 'language': 'es', + 'model_repo': 'ggerganov/whisper.cpp', + 'model_filename': 'ggml-large-v3-turbo-q5_0.bin', + 'use_gpu': 'False',} + .items() + ) + + audio_common_player_cmd = Node( + package='audio_common', + executable='audio_player_node', + parameters=[ + {'channels': 1}, + {'device': -1}] + ) + # tts_model = 'tts_models/en/ljspeech/glow-tts' + # tts_model = 'tts_models/es/mai/tacotron2-DDC' + tts_model = 'tts_models/es/css10/vits' + + audio_common_tts_cmd = Node( + package='tts_ros', + executable='tts_node', + parameters=[ + {'chunk': 4096}, + {'frame_id': ''}, + {'model': tts_model}, + {'speaker_wav': ''}, + {'device': 'cpu'}] + ) + + ld = LaunchDescription() + + if whisper: + ld.add_action(whisper_cmd) + if llama: + ld.add_action(llama_cmd) + ld.add_action(audio_common_player_cmd) + ld.add_action(audio_common_tts_cmd) + + return ld + diff --git a/hri/package.xml b/hri/package.xml new file mode 100644 index 0000000..83f59ed --- /dev/null +++ b/hri/package.xml @@ -0,0 +1,29 @@ + + + + hri_bt_nodes + 0.0.0 + TODO: Package description + Samuele + rod + Apache License, Version 2.0 + ament_cmake + + rclcpp + rclcpp_cascade_lifecycle + rclcpp_action + behaviortree_cpp_v3 + audio_common_msgs + whisper_msgs + llama_msgs + std_msgs + ament_index_cpp + libzmq3-dev + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hri/src/hri/dialog/dialog_confirmation.cpp b/hri/src/hri/dialog/dialog_confirmation.cpp new file mode 100644 index 0000000..30d3a9b --- /dev/null +++ b/hri/src/hri/dialog/dialog_confirmation.cpp @@ -0,0 +1,82 @@ +// 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 "hri/dialog/dialog_confirmation.hpp" + + + +namespace hri +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; + +DialogConfirmation::DialogConfirmation( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: hri::BtActionNode( + xml_tag_name, action_name, conf) +{ + speech_start_publisher_ = node_->create_publisher("dialog_action", 10); + + getInput("language", lang_); +} + +void DialogConfirmation::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "DialogConfirmation ticked"); + std::string text; + + goal_ = whisper_msgs::action::STT::Goal(); + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg_dialog_action.data = START_LISTENING_; + + speech_start_publisher_->publish(msg_dialog_action); +} + +BT::NodeStatus DialogConfirmation::on_success() +{ + RCLCPP_INFO(node_->get_logger(), "I heard: %s", result_.result->transcription.text.c_str()); + + if (result_.result->transcription.text.size() == 0) { + return BT::NodeStatus::FAILURE; + } + + std::transform( + result_.result->transcription.text.begin(), result_.result->transcription.text.end(), result_.result->transcription.text.begin(), + [](unsigned char c) {return std::tolower(c);}); + + std::string yes_word = "yes"; + if (lang_ == "es") { + yes_word = "sí"; + } + + if (result_.result->transcription.text.find(yes_word) != std::string::npos) { + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::FAILURE; + } +} + +} // namespace hri +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "/whisper/listen", config); + }; + + factory.registerBuilder("DialogConfirmation", builder); +} diff --git a/hri/src/hri/dialog/listen.cpp b/hri/src/hri/dialog/listen.cpp new file mode 100644 index 0000000..83f0b69 --- /dev/null +++ b/hri/src/hri/dialog/listen.cpp @@ -0,0 +1,67 @@ +// 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 "hri/dialog/listen.hpp" + +namespace dialog +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; + +Listen::Listen( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: hri::BtActionNode( + xml_tag_name, action_name, conf) +{ + listen_start_publisher_ = node_->create_publisher("dialog_phase", 10); +} + +void +Listen::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "Listen ticked"); + goal_ = whisper_msgs::action::STT::Goal(); + auto msg_dialog_action = std_msgs::msg::Int8(); + + msg_dialog_action.data = START_LISTENING_; + + listen_start_publisher_->publish(msg_dialog_action); +} + +BT::NodeStatus Listen::on_success() +{ + + RCLCPP_INFO(node_->get_logger(), "I heard: %s", result_.result->transcription.text.c_str()); + + if (result_.result->transcription.text.size() == 0) { + return BT::NodeStatus::FAILURE; + } + + setOutput("listened_text", result_.result->transcription.text); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace dialog +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "whisper/listen", config); + }; + + factory.registerBuilder("Listen", builder); +} diff --git a/hri/src/hri/dialog/query.cpp b/hri/src/hri/dialog/query.cpp new file mode 100644 index 0000000..50b5877 --- /dev/null +++ b/hri/src/hri/dialog/query.cpp @@ -0,0 +1,155 @@ +// 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 "hri/dialog/query.hpp" + +namespace dialog +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; +using json = nlohmann::json; + +Query::Query( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: hri::BtActionNode< + llama_msgs::action::GenerateResponse, rclcpp_cascade_lifecycle::CascadeLifecycleNode>( + xml_tag_name, action_name, conf) +{ + + node_->declare_parameter("prompt_file", prompt_file_path_); + node_->declare_parameter("grammar_file", grammar_file_path_); + node_->get_parameter("prompt_file", prompt_file_path_); + node_->get_parameter("grammar_file", grammar_file_path_); + + if (!node_->has_parameter("placeholder")) { + node_->declare_parameter("placeholder", placeholder_); + } + node_->get_parameter("placeholder", placeholder_); + + try { + std::string pkg_dir = ament_index_cpp::get_package_share_directory("hri_bt_nodes"); + prompt_file_path_ = pkg_dir + "/config/" + prompt_file_path_; + grammar_file_path_ = pkg_dir + "/config/" + grammar_file_path_; + } catch (const std::exception &e) { + RCLCPP_ERROR(node_->get_logger(), "Error getting package share directory: %s", e.what()); + return; + } + +} + +std::string +Query::load_text_file(std::string path) +{ + std::ifstream file(path); + if (!file.is_open()) { + return ""; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + file.close(); + + return buffer.str(); + +} + +std::string +Query::swap_placeholders(std::string text, std::vector elems) +{ + for (const auto& elem : elems) { + size_t pos = text.find(placeholder_); + if (pos != std::string::npos) { + text.replace(pos, placeholder_.length(), elem); + RCLCPP_DEBUG(node_->get_logger(), "Modified string: %s", text.c_str()); + } else { + RCLCPP_ERROR(node_->get_logger(), "Placeholder %s not found in prompt", placeholder_.c_str()); + break; + } + } + + return text; +} + +void Query::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "Query ticked"); + + std::string text, intention; + getInput("text", text); + getInput("intention", intention); + + std::string prompt = load_text_file(prompt_file_path_); + if (prompt.length() == 0) { + RCLCPP_ERROR(node_->get_logger(), "Error loading the prompt"); + return; + } + + std::vector params = {text, intention}; + + prompt = swap_placeholders(prompt, params); + if (prompt.length() == 0) { + RCLCPP_ERROR(node_->get_logger(), "Error loading the prompt"); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Prompt: %s", prompt.c_str()); + + goal_.prompt = prompt; + goal_.reset = true; + goal_.sampling_config.temp = 0.0; + goal_.sampling_config.grammar = load_text_file(grammar_file_path_); + + if (goal_.sampling_config.grammar.length() == 0) { + RCLCPP_ERROR(node_->get_logger(), "Error loading the grammar"); + return; + } + + RCLCPP_DEBUG(node_->get_logger(), "Grammar: %s", goal_.sampling_config.grammar.c_str()); + +} + +BT::NodeStatus Query::on_success() +{ + + RCLCPP_INFO(node_->get_logger(), "Response: %s", result_.result->response.text.c_str()); + + if (result_.result->response.text.empty() || result_.result->response.text == "{}") { + return BT::NodeStatus::FAILURE; + } + + json response = json::parse(result_.result->response.text); + std::string value = response["intention"]; + + if (value.empty()) { + return BT::NodeStatus::FAILURE; + } + + setOutput("intention_value", value); + RCLCPP_INFO(node_->get_logger(), "Intention value: %s", value.c_str()); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace dialog +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "/llama/generate_response", config); + }; + + factory.registerBuilder("Query", builder); +} diff --git a/hri/src/hri/dialog/speak.cpp b/hri/src/hri/dialog/speak.cpp new file mode 100644 index 0000000..0dd3af6 --- /dev/null +++ b/hri/src/hri/dialog/speak.cpp @@ -0,0 +1,119 @@ +// 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 "hri/dialog/speak.hpp" + +namespace dialog +{ +using namespace std::chrono_literals; +using namespace std::placeholders; + +Speak::Speak( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: hri::BtActionNode( + xml_tag_name, action_name, conf) +{ + config().blackboard->get("node", node_); + + if (!node_->has_parameter("placeholder")) { + node_->declare_parameter("placeholder", placeholder_); + } + node_->get_parameter("placeholder", placeholder_); + + RCLCPP_DEBUG(node_->get_logger(), "Placeholder: %s", placeholder_.c_str()); + + speech_text_publisher_ = node_->create_publisher("speech_text", 10); + speech_start_publisher_ = node_->create_publisher("dialog_phase", 10); + + speech_text_publisher_->on_activate(); + speech_start_publisher_->on_activate(); + +} + +std::string +Speak::swap_placeholders(std::string text, std::vector elems) +{ + for (const auto& elem : elems) { + size_t pos = text.find(placeholder_); + if (pos != std::string::npos) { + text.replace(pos, placeholder_.length(), elem); + RCLCPP_DEBUG(node_->get_logger(), "Modified string: %s", text.c_str()); + } else { + RCLCPP_ERROR(node_->get_logger(), "Placeholder %s not found in prompt", placeholder_.c_str()); + break; + } + } + + return text; +} + + +void +Speak::on_tick() +{ + RCLCPP_INFO(node_->get_logger(), "SPEAK"); + rclcpp::spin_some(node_->get_node_base_interface()); + + + goal_ = audio_common_msgs::action::TTS::Goal(); + std::string text; + getInput("speech_text", text); + + std::vector params; + std::string sparams; + getInput("params", sparams); + + std::stringstream ss(sparams); + std::string item; + std::vector result; + while (std::getline(ss, item, ';')) { + RCLCPP_INFO(node_->get_logger(), "Param: %s", item.c_str()); + params.push_back(item); + } + RCLCPP_INFO(node_->get_logger(), "Text: %s", text.c_str()); + + text = swap_placeholders(text, params); + + goal_.text = text; + RCLCPP_INFO(node_->get_logger(), "Sending goal. Text: %s", goal_.text.c_str()); + + auto speech_text_msg_ = std_msgs::msg::String(); + auto speech_start_msg_ = std_msgs::msg::Int8(); + + speech_text_msg_.data = goal_.text; + speech_start_msg_.data = START_SPEECH_; + + speech_text_publisher_->publish(speech_text_msg_); + speech_start_publisher_->publish(speech_start_msg_); + +} + +BT::NodeStatus +Speak::on_success() +{ + return BT::NodeStatus::SUCCESS; +} + +} // namespace dialog + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "say", config); + }; + + factory.registerBuilder("Speak", builder); +} diff --git a/hri/src/main_hri.cpp b/hri/src/main_hri.cpp new file mode 100644 index 0000000..6651db2 --- /dev/null +++ b/hri/src/main_hri.cpp @@ -0,0 +1,75 @@ +// 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 +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("hri_node"); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + std::vector plugins; + std::string bt_xml_file; + node->declare_parameter("plugins", plugins); + node->declare_parameter("bt_xml_file", bt_xml_file); + node->get_parameter("plugins", plugins); + node->get_parameter("bt_xml_file", bt_xml_file); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + for (const auto & plugin : plugins) { + RCLCPP_INFO(node->get_logger(), "Loading BT Node: [%s]", plugin.c_str()); + factory.registerFromPlugin(loader.getOSName(plugin)); + } + + std::string pkgpath = ament_index_cpp::get_package_share_directory("hri_bt_nodes"); + std::string xml_file = pkgpath + "/bt_xml/" + bt_xml_file; + + RCLCPP_INFO(node->get_logger(), "Loading BT: [%s]", xml_file.c_str()); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", node); + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + + RCLCPP_INFO(node->get_logger(), "Starting behavior from BT"); + + auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); + + rclcpp::Rate rate(30); + + bool finish = false; + while (!finish && rclcpp::ok()) { + finish = tree.tickRoot() != BT::NodeStatus::RUNNING; + rclcpp::spin_some(node->get_node_base_interface()); + rate.sleep(); + } + + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + + rclcpp::shutdown(); + return 0; +} diff --git a/hri/thirdparty.repos b/hri/thirdparty.repos new file mode 100644 index 0000000..832b973 --- /dev/null +++ b/hri/thirdparty.repos @@ -0,0 +1,21 @@ +repositories: + thirdparty/cascade_lifecycle: + type: jazzy-devel + url: https://github.com/fmrico/cascade_lifecycle + version: main + thirdparty/whisper_ros: + type: git + url: https://github.com/mgonzs13/whisper_ros.git + version: main + thirdparty/audio_common: + type: git + url: https://github.com/mgonzs13/audio_common.git + version: main + thirdparty/tts_ros: + type: git + url: https://github.com/mgonzs13/tts_ros.git + version: main + thirdparty/llama_ros: + type: git + url: https://github.com/mgonzs13/llama_ros.git + version: main \ No newline at end of file diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt new file mode 100644 index 0000000..5209362 --- /dev/null +++ b/motion/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.8) +project(motion_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(rclcpp REQUIRED) +find_package(rclcpp_cascade_lifecycle REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(behaviortree_cpp_v3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + rclcpp + rclcpp_cascade_lifecycle + rclcpp_action + behaviortree_cpp_v3 + geometry_msgs + nav2_msgs + tf2_ros + tf2_geometry_msgs + tf2 + ament_index_cpp + trajectory_msgs + sensor_msgs +) + +include_directories(include) + +add_library(navigate_to_bt_node SHARED src/motion/navigation/navigate_to.cpp) +add_library(navigate_through_bt_node SHARED src/motion/navigation/navigate_through.cpp) +add_library(spin_bt_node SHARED src/motion/base/spin.cpp) +add_library(face_bt_node SHARED src/motion/base/face.cpp) +add_library(pan_bt_node SHARED src/motion/head/pan.cpp) + +list(APPEND + plugin_libs + navigate_to_bt_node + navigate_through_bt_node + spin_bt_node + face_bt_node + pan_bt_node +) + +foreach(bt_plugin ${plugin_libs}) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +endforeach() + +add_executable(motion_test src/main_motion.cpp) +ament_target_dependencies(motion_test ${dependencies}) +target_link_libraries(motion_test ${ZMQ_LIBRARIES}) + +install(TARGETS + motion_test + ${plugin_libs} + EXPORT 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) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) + +foreach(bt_plugin ${plugin_libs}) + ament_export_libraries(${bt_plugin}) +endforeach() + +ament_package() diff --git a/motion/README.md b/motion/README.md new file mode 100644 index 0000000..92335a3 --- /dev/null +++ b/motion/README.md @@ -0,0 +1,19 @@ +# motion_bt_nodes +A collection of BT nodes for motion capabilities + +## How to test it in [tiago_harmonic](https://github.com/Tiago-Harmonic/tiago_harmonic) simulator + +### Run de simulator +```bash +ros2 launch tiago_gazebo tiago_gazebo.launch.py is_public_sim:=True world_name:=house +``` + +### Activate navigation +```bash +ros2 launch tiago_2dnav tiago_nav_bringup.launch.py is_public_sim:=True world_name:=small_house +``` + +### Run the example +```bash +ros2 launch motion_bt_nodes nav.launch.py +``` diff --git a/motion/bt_xml/face.xml b/motion/bt_xml/face.xml new file mode 100644 index 0000000..b138db7 --- /dev/null +++ b/motion/bt_xml/face.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 1: left; -1: right + + + + + diff --git a/motion/bt_xml/face_spin.xml b/motion/bt_xml/face_spin.xml new file mode 100644 index 0000000..f9d995d --- /dev/null +++ b/motion/bt_xml/face_spin.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1: left; -1: right + + + + + diff --git a/motion/bt_xml/nav.xml b/motion/bt_xml/nav.xml new file mode 100644 index 0000000..c08a2fc --- /dev/null +++ b/motion/bt_xml/nav.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/nav_through.xml b/motion/bt_xml/nav_through.xml new file mode 100644 index 0000000..96b5d53 --- /dev/null +++ b/motion/bt_xml/nav_through.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/nav_truncate.xml b/motion/bt_xml/nav_truncate.xml new file mode 100644 index 0000000..5359d68 --- /dev/null +++ b/motion/bt_xml/nav_truncate.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/spin.xml b/motion/bt_xml/spin.xml new file mode 100644 index 0000000..29b1c50 --- /dev/null +++ b/motion/bt_xml/spin.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/spin_time.xml b/motion/bt_xml/spin_time.xml new file mode 100644 index 0000000..90b8f45 --- /dev/null +++ b/motion/bt_xml/spin_time.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/motion/config/motion.yaml b/motion/config/motion.yaml new file mode 100644 index 0000000..ed56dd8 --- /dev/null +++ b/motion/config/motion.yaml @@ -0,0 +1,9 @@ +nav_node: + ros__parameters: + use_sim_time: True + bt_xml_file: spin.xml + plugins: + - navigate_to_bt_node + - navigate_through_bt_node + - spin_bt_node + - get_angle_bt_node diff --git a/motion/include/ctrl_support/BTActionNode.hpp b/motion/include/ctrl_support/BTActionNode.hpp new file mode 100644 index 0000000..98c3056 --- /dev/null +++ b/motion/include/ctrl_support/BTActionNode.hpp @@ -0,0 +1,275 @@ +// Copyright (c) 2018 Intel Corporation +// +// 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 NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ +#define NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/action_node.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace motion +{ + +using namespace std::chrono_literals; // NOLINT + +template +class BtActionNode : public BT::ActionNodeBase +{ +public: + BtActionNode( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) + : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) + { + node_ = config().blackboard->get("node"); + + server_timeout_ = 1s; + + // Initialize the input and output messages + goal_ = typename ActionT::Goal(); + result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); + + std::string remapped_action_name; + if (getInput("server_name", remapped_action_name)) { + action_name_ = remapped_action_name; + } + createActionClient(action_name_); + + // Give the derive class a chance to do any initialization + RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); + } + + BtActionNode() = delete; + + virtual ~BtActionNode() {} + + // Create instance of an action server + void createActionClient(const std::string & action_name) + { + // Now that we have the ROS node to use, create the action client for this BT action + action_client_ = rclcpp_action::create_client(node_, action_name); + + // Make sure the server is actually there before continuing + RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + action_client_->wait_for_action_server(); + } + + // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method + // and call providedBasicPorts in it. + static BT::PortsList providedBasicPorts(BT::PortsList addition) + { + BT::PortsList basic = { + BT::InputPort("server_name", "Action server name"), + BT::InputPort("server_timeout")}; + basic.insert(addition.begin(), addition.end()); + + return basic; + } + + static BT::PortsList providedPorts() {return providedBasicPorts({});} + + // Derived classes can override any of the following methods to hook into the + // processing for the action: on_tick, on_wait_for_result, and on_success + + // Could do dynamic checks, such as getting updates to values on the blackboard + virtual void on_tick() {} + + // There can be many loop iterations per tick. Any opportunity to do something after + // a timeout waiting for a result that hasn't been received yet + virtual void on_wait_for_result() {} + + // Called upon successful completion of the action. A derived class can override this + // method to put a value on the blackboard, for example. + virtual BT::NodeStatus on_success() {return BT::NodeStatus::SUCCESS;} + + // Called when a the action is aborted. By default, the node will return FAILURE. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_aborted() {return BT::NodeStatus::FAILURE;} + + // Called when a the action is cancelled. By default, the node will return SUCCESS. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_cancelled() {return BT::NodeStatus::SUCCESS;} + + // The main override required by a BT action + BT::NodeStatus tick() override + { + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) { + createActionClient(action_name_); + + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + + // user defined callback + try { + on_tick(); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + return BT::NodeStatus::FAILURE; + } + + on_new_goal_received(); + } + + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" + on_wait_for_result(); + + auto goal_status = goal_handle_->get_status(); + if ( + goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + on_new_goal_received(); + } + + rclcpp::spin_some(node_->get_node_base_interface()); + + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } + } + + switch (result_.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + return on_success(); + + case rclcpp_action::ResultCode::ABORTED: + return on_aborted(); + + case rclcpp_action::ResultCode::CANCELED: + return on_cancelled(); + + default: + throw std::logic_error("BtActionNode::Tick: invalid status value"); + } + } + + // The other (optional) override required by a BT action. In this case, we + // make sure to cancel the ROS2 action if it is still running. + void halt() override + { + if (should_cancel_goal()) { + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + if ( + rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); + } + } + + setStatus(BT::NodeStatus::IDLE); + } + +protected: + bool should_cancel_goal() + { + // Shut the node down if it is currently running + if (status() != BT::NodeStatus::RUNNING) { + return false; + } + + rclcpp::spin_some(node_->get_node_base_interface()); + auto status = goal_handle_->get_status(); + + // Check if the goal is still executing + return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; + } + + void on_new_goal_received() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + // TODO(#1652): a work around until rcl_action interface is updated + // if goal ids are not matched, the older goal call this callback so ignore the result + // if matched, it must be processed (including aborted) + if (this->goal_handle_->get_goal_id() == result.goal_id) { + goal_result_available_ = true; + result_ = result; + } + }; + + send_goal_options.feedback_callback = std::bind( + &BtActionNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + + if ( + rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("send_goal failed"); + } + + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + } + + void feedback_callback( + typename rclcpp_action::ClientGoalHandle::SharedPtr, + const typename ActionT::Feedback::ConstSharedPtr feedback) + { + // config().blackboard->set("feedback", feedback); + feedback_ = *feedback; + on_feedback(); + } + + virtual void on_feedback() {} + + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + + std::string action_name_; + typename std::shared_ptr> action_client_; + + // All ROS2 actions have a goal and a result + typename ActionT::Goal goal_; + typename ActionT::Feedback feedback_; + bool goal_updated_{false}; + bool goal_result_available_{false}; + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + typename rclcpp_action::ClientGoalHandle::WrappedResult result_; + // The node that will be used for any ROS operations + typename NodeT::SharedPtr node_; + + // The timeout value while waiting for response from a server when a + // new action goal is sent or canceled + std::chrono::milliseconds server_timeout_; +}; + +} // namespace motion + +#endif // NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ \ No newline at end of file diff --git a/motion/include/motion/base/face.hpp b/motion/include/motion/base/face.hpp new file mode 100644 index 0000000..bb66238 --- /dev/null +++ b/motion/include/motion/base/face.hpp @@ -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. + +#ifndef BASE_FACE__HPP_ +#define BASE_FACE__HPP_ + +#include +#include +#include + +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "motion/navigation/utils.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +namespace base +{ + +class Face : public motion::BtActionNode< + nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit Face( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + BT::NodeStatus on_aborted() override; + BT::NodeStatus on_cancelled() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("tf_frame"), + BT::InputPort("base_frame"), + }); + } + +private: + std::shared_ptr node_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_executor_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::string base_frame_, tf_frame_; +}; + +} // namespace navigation + +#endif // BASE_FACE__HPP_ diff --git a/motion/include/motion/base/spin.hpp b/motion/include/motion/base/spin.hpp new file mode 100644 index 0000000..8b08307 --- /dev/null +++ b/motion/include/motion/base/spin.hpp @@ -0,0 +1,69 @@ +// 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 BASE_ROTATE_HPP_ +#define BASE_ROTATE_HPP_ + +#include + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/twist.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/string.hpp" + +namespace base +{ + +class Spin : public BT::ActionNodeBase +{ +public: + explicit Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + void halt(); + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("angle"), // if negative, rotate right, if positive, rotate left + BT::InputPort("speed"), // rad/s + BT::InputPort("forever") // if true, keep rotating + }); + } + +private: + std::shared_ptr node_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; + + double angle_, rotated_angle_, speed_; + bool forever_; + // int direction_; + + std::chrono::time_point last_time_; +}; + +} // namespace base + +#endif // BASE_ROTATE_HPP_ diff --git a/motion/include/motion/head/pan.hpp b/motion/include/motion/head/pan.hpp new file mode 100644 index 0000000..4b24b96 --- /dev/null +++ b/motion/include/motion/head/pan.hpp @@ -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. + +#ifndef HEAD__PAN_HPP_ +#define HEAD__PAN_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace head +{ + +class Pan : public BT::ActionNodeBase +{ +public: + explicit Pan(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + void halt(); + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("range"), // in degrees + BT::InputPort("period"), // in seconds + BT::InputPort("pitch_angle") // in degrees + }); + } + +private: + // std::shared_ptr node_; + std::shared_ptr node_; + rclcpp::Time start_time_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + joint_cmd_pub_; + double yaw_limit_{1.3}; + double pitch_limit_{0.92}; + double pitch_{0.0}; + std::vector yaw_positions_{0.0, 0.7, 0.7, 0.7, 0.7, -0.7, -0.7, 0.0}; + std::vector pitch_positions_{0.0, 0.0, 0.3, -0.3, 0.3, 0.3, -0.3, 0.0}; + std::vector times_from_start_{0.1, 3.0, 6.0, 9.0, 12.0, 15.0, 18.0, 21.0}; + rclcpp::Subscription::SharedPtr joint_state_sub_; + double joint_range_, period_; + double pitch_angle_ = 0.0; + double phase_; + + int current_position_{0}; + + double get_joint_yaw(double period, double range, double time, double phase); +}; + +} // namespace head + +#endif // HEAD__PAN_HPP_ diff --git a/motion/include/motion/navigation/navigate_through.hpp b/motion/include/motion/navigation/navigate_through.hpp new file mode 100644 index 0000000..389c930 --- /dev/null +++ b/motion/include/motion/navigation/navigate_through.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 NAVIGATION__NAVIGATE_THROUGH_HPP_ +#define NAVIGATION__NAVIGATE_THROUGH_HPP_ + +#include + +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/action/navigate_through_poses.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +namespace navigation +{ + +class NavigateThrough : public motion::BtActionNode< + nav2_msgs::action::NavigateThroughPoses, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit NavigateThrough( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + BT::NodeStatus on_aborted() override; + BT::NodeStatus on_cancelled() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("tf_frame"), + BT::InputPort("style"), // straight, circular-l, circular-r + BT::InputPort("n_poses"), + BT::InputPort("k_radius"), + BT::InputPort("x"), + BT::InputPort("y"), + }); + } + +private: + std::vector generate_poses_to_goal( + const geometry_msgs::msg::PoseStamped & goal, + geometry_msgs::msg::TransformStamped map_to_goal, + geometry_msgs::msg::TransformStamped map_to_robot, + int n_poses, const std::string & style); + + std::shared_ptr node_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + double distance_tolerance_; + +}; + +} // namespace navigation + +#endif // NAVIGATION__NAVIGATE_THROUGH_HPP_ diff --git a/motion/include/motion/navigation/navigate_to.hpp b/motion/include/motion/navigation/navigate_to.hpp new file mode 100644 index 0000000..6875957 --- /dev/null +++ b/motion/include/motion/navigation/navigate_to.hpp @@ -0,0 +1,85 @@ +// 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 NAVIGATION__NAVIGATE_TO_HPP_ +#define NAVIGATION__NAVIGATE_TO_HPP_ + +#include +#include +#include + +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "motion/navigation/utils.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +// #include "nav2_msgs/action/compute_path_to_pose.hpp" +// #include "nav2_msgs/action/follow_path.hpp" +// #include "nav2_util/geometry_utils.hpp" +// #include "rclcpp_action/rclcpp_action.hpp" + +namespace navigation +{ + +class NavigateTo : public motion::BtActionNode< + nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode> +{ +public: + explicit NavigateTo( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf); + + void on_tick() override; + BT::NodeStatus on_success() override; + BT::NodeStatus on_aborted() override; + BT::NodeStatus on_cancelled() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("distance_tolerance"), + BT::InputPort("tf_frame"), + BT::InputPort("x"), + BT::InputPort("y"), + BT::InputPort("will_finish") + }); + } + +private: + std::shared_ptr node_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_executor_; + + // tf2::BufferCore tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + bool will_finish_{true}; + +}; + +} // namespace navigation + +#endif // NAVIGATION__NAVIGATE_TO_HPP_ diff --git a/motion/include/motion/navigation/utils.hpp b/motion/include/motion/navigation/utils.hpp new file mode 100644 index 0000000..c100600 --- /dev/null +++ b/motion/include/motion/navigation/utils.hpp @@ -0,0 +1,120 @@ +// Copyright 2024 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 NAVIGATION__UTILS_HPP_ +#define NAVIGATION__UTILS_HPP_ + +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace navigation +{ + +std::string nav_to_pose_truncated_xml = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +std::string dynamic_following_xml = + R"( + + + + + + + + + + + + + + + + + + + + + +)"; + +std::string modify_truncate_distance(std::string xml, double distance) +{ + std::string distance_str = std::to_string(distance); + std::string to_find = "distance=\""; + std::size_t found = xml.find(to_find); + if (found != std::string::npos) { + std::size_t start = found + to_find.size(); + std::size_t end = xml.find("\"", start); + xml.replace(start, end - start, distance_str); + } + return xml; +} + +// generate a temp file with the given content, as input the distance to truncate the path +inline std::string generate_xml_file(std::string content, double distance) +{ + std::string temp_file = "/tmp/bt_xml_XXXXXX"; + char * temp_file_c = new char[temp_file.length() + 1]; + strcpy(temp_file_c, temp_file.c_str()); + int fd = mkstemp(temp_file_c); + if (fd == -1) { + std::cerr << "Error creating temp file" << std::endl; + return ""; + } + std::ofstream file(temp_file_c); + file << modify_truncate_distance(content, distance); + file.close(); + return std::string(temp_file_c); +} + +} // namespace navigation + +#endif // NAVIGATION__UTILS_HPP_ diff --git a/motion/launch/motion.launch.py b/motion/launch/motion.launch.py new file mode 100644 index 0000000..7a58431 --- /dev/null +++ b/motion/launch/motion.launch.py @@ -0,0 +1,50 @@ +# 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('motion_bt_nodes') + + params_file = os.path.join(pkg_dir, 'config', 'motion.yaml') + # print('params_file: ', params_file) + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['nav_node']['ros__parameters'] + # print(params) + + ld = LaunchDescription() + + nav_cmd = Node( + package='motion_bt_nodes', + executable='motion_test', + output='screen', + remappings=[ + ], + parameters=[{ + 'use_sim_time': True, + }, params] + ) + + ld.add_action(nav_cmd) + + return ld + diff --git a/motion/package.xml b/motion/package.xml new file mode 100644 index 0000000..9ae3295 --- /dev/null +++ b/motion/package.xml @@ -0,0 +1,33 @@ + + + + motion_bt_nodes + 0.0.0 + TODO: Package description + rod + Juanca + Alberto + Apache License, Version 2.0 + ament_cmake + + rclcpp + rclcpp_cascade_lifecycle + rclcpp_action + behaviortree_cpp_v3 + geometry_msgs + nav2_msgs + navigation_system_interfaces + tf2_ros + tf2_geometry_msgs + tf2 + + ament_lint_auto + ament_lint_common + + + + ament_cmake + + + + diff --git a/motion/src/main_motion.cpp b/motion/src/main_motion.cpp new file mode 100644 index 0000000..6deffa8 --- /dev/null +++ b/motion/src/main_motion.cpp @@ -0,0 +1,77 @@ +// 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 +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("nav_node"); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + std::vector plugins; + std::string bt_xml_file; + node->declare_parameter("plugins", plugins); + node->declare_parameter("bt_xml_file", bt_xml_file); + node->get_parameter("plugins", plugins); + node->get_parameter("bt_xml_file", bt_xml_file); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + for (const auto & plugin : plugins) { + RCLCPP_INFO(node->get_logger(), "Loading BT Node: [%s]", plugin.c_str()); + factory.registerFromPlugin(loader.getOSName(plugin)); + } + + std::string pkgpath = ament_index_cpp::get_package_share_directory("motion_bt_nodes"); + std::string xml_file = pkgpath + "/bt_xml/" + bt_xml_file; + + RCLCPP_INFO(node->get_logger(), "Loading BT: [%s]", xml_file.c_str()); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", node); + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + + RCLCPP_INFO(node->get_logger(), "Starting behavior from BT"); + + auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); + + rclcpp::Rate rate(30); + + bool finish = false; + while (!finish && rclcpp::ok()) { + finish = tree.tickRoot() != BT::NodeStatus::RUNNING; + rclcpp::spin_some(node->get_node_base_interface()); + rate.sleep(); + } + + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + + rclcpp::shutdown(); + return 0; +} diff --git a/motion/src/motion/base/face.cpp b/motion/src/motion/base/face.cpp new file mode 100644 index 0000000..b175d48 --- /dev/null +++ b/motion/src/motion/base/face.cpp @@ -0,0 +1,99 @@ +// 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 "motion/base/face.hpp" + +namespace base +{ + +Face::Face( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: motion::BtActionNode( + xml_tag_name, action_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + getInput("base_frame", base_frame_); + getInput("tf_frame", tf_frame_); +} + +void Face::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "Face ticked"); + geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::TransformStamped robot_to_goal, map_to_robot; + + RCLCPP_INFO(node_->get_logger(), "Transforming %s to %s", base_frame_.c_str(), tf_frame_.c_str()); + try { + robot_to_goal = tf_buffer_.lookupTransform(base_frame_, tf_frame_, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN( + node_->get_logger(), "Could not transform %s to %s: %s", base_frame_.c_str(), tf_frame_.c_str(), ex.what()); + setStatus(BT::NodeStatus::RUNNING); + } + + // RCLCPP_INFO(node_->get_logger(), "Transforming map to %s", base_frame_.c_str()); + // try { + // map_to_robot = tf_buffer_.lookupTransform("map", base_frame_, tf2::TimePointZero); + // } catch (const tf2::TransformException & ex) { + // RCLCPP_WARN( + // node_->get_logger(), "Could not transform map to %s: %s", base_frame_.c_str(), ex.what()); + // setStatus(BT::NodeStatus::RUNNING); + // } + + goal.header.stamp = node_->now(); + goal.header.frame_id = base_frame_; + + // Set position to the robot's current position + // goal.pose.position.x = map_to_robot.transform.translation.x; + // goal.pose.position.y = map_to_robot.transform.translation.y; + // goal.pose.position.z = map_to_robot.transform.translation.z; + goal.pose.position.x = 0; + goal.pose.position.y = 0; + goal.pose.position.z = 0; + + // Calculate the yaw angle to face the target frame + double dx = robot_to_goal.transform.translation.x; + double dy = robot_to_goal.transform.translation.y; + double yaw = std::atan2(dy, dx); // Angle to face the target frame + + // Convert yaw to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, yaw); // Roll and pitch are zero; yaw is computed + + goal.pose.orientation.x = q.x(); + goal.pose.orientation.y = q.y(); + goal.pose.orientation.z = q.z(); + goal.pose.orientation.w = q.w(); + + RCLCPP_INFO(node_->get_logger(), "Robot will rotate to face %s (yaw=%.2f radians)", tf_frame_.c_str(), yaw); + + goal_.pose = goal; +} + + +} // namespace base + +#include "behaviortree_cpp_v3/bt_factory.h" + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "navigate_to_pose", config); + }; + + factory.registerBuilder("Face", builder); +} diff --git a/motion/src/motion/base/spin.cpp b/motion/src/motion/base/spin.cpp new file mode 100644 index 0000000..a4c21cc --- /dev/null +++ b/motion/src/motion/base/spin.cpp @@ -0,0 +1,91 @@ +// 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 "motion/base/spin.hpp" + +namespace base +{ + +Spin::Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + config().blackboard->get("node", node_); + + vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_->on_activate(); +} + +BT::NodeStatus Spin::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "SPIN"); + + getInput("angle", angle_); + getInput("speed", speed_); + getInput("forever", forever_); + // getInput("direction", direction_); + + if (forever_) { + RCLCPP_INFO_ONCE(node_->get_logger(), "Spinning forever"); + } else { + // RCLCPP_INFO(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s (dir: %d)", angle_, speed_, direction_); + RCLCPP_INFO_ONCE(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s", angle_, speed_); + } + + if (status() == BT::NodeStatus::IDLE) { + rotated_angle_ = 0; + last_time_ = std::chrono::high_resolution_clock::now(); + std::time_t current_time = std::chrono::system_clock::to_time_t(last_time_); + std::tm local_time = *std::localtime(¤t_time); + RCLCPP_DEBUG(node_->get_logger(), "Starting to spin at: %d:%d:%d", local_time.tm_hour, local_time.tm_min, local_time.tm_sec); + return BT::NodeStatus::RUNNING; + } + + geometry_msgs::msg::Twist cmd_vel; + // cmd_vel.angular.z = speed_ * direction_; + cmd_vel.angular.z = speed_ * std::copysign(1, angle_); + + if (forever_) { + RCLCPP_DEBUG(node_->get_logger(), "Spinning forever"); + if (angle_ != 0) { + vel_pub_->publish(cmd_vel); + } + return BT::NodeStatus::RUNNING; + } else if ((std::abs(rotated_angle_) < std::abs(angle_))) { + // RCLCPP_DEBUG(node_->get_logger(), "Target: %.2f degrees (dir: %d). Spinned %.2f degrees: ", angle_, direction_, rotated_angle_); + RCLCPP_INFO(node_->get_logger(), "Target: %.2f degrees. Spinned %.2f degrees: ", angle_, rotated_angle_); + vel_pub_->publish(cmd_vel); + auto curr_time = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::duration_cast(curr_time - last_time_); + rotated_angle_ += (speed_ * elapsed.count() / 1000.0) * 180 / M_PI; + last_time_ = curr_time; + return BT::NodeStatus::RUNNING; + } + + // std::time_t current_time = std::chrono::system_clock::to_time_t(last_time_); + // std::tm local_time = *std::localtime(¤t_time); + // RCLCPP_INFO(node_->get_logger(), "Finished spinning %.2f degrees at %d:%d:%d", angle_, local_time.tm_hour, local_time.tm_min, local_time.tm_sec); + RCLCPP_INFO(node_->get_logger(), "Finished spinning %.2f degrees", angle_); + return BT::NodeStatus::SUCCESS; +} + +void Spin::halt() +{ + RCLCPP_DEBUG(node_->get_logger(), "Spin halted"); +} + +} // namespace base + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Spin"); +} diff --git a/motion/src/motion/head/pan.cpp b/motion/src/motion/head/pan.cpp new file mode 100644 index 0000000..b9db0b9 --- /dev/null +++ b/motion/src/motion/head/pan.cpp @@ -0,0 +1,113 @@ +// 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 "motion/head/pan.hpp" + +namespace head +{ + +using namespace std::chrono_literals; + +Pan::Pan(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf), phase_(0.0) +{ + config().blackboard->get("node", node_); + // joint_range_ = 20.0 * M_PI / 180.0; + getInput("range", joint_range_); + joint_range_ = joint_range_ * M_PI / 180.0; + getInput("period", period_); + getInput("pitch_angle", pitch_angle_); + pitch_angle_ = pitch_angle_ * M_PI / 180.0; + + // if (!joint_range_) { + // // throw BT::RuntimeError("Missing required input [range]: ", joint_range_); + // RCLCPP_WARN( + // node_->get_logger(), "Missing required input [range]. Using default value 45.0 degrees"); + // joint_range_.value() = 45.0 * M_PI / 180.0; + // } + // if (!period_) { + // // throw BT::RuntimeError("Missing required input [period]: ", period_); + // RCLCPP_WARN( + // node_->get_logger(), "Missing required input [period]. Using default value 5.0 seconds"); + // period_.value() = 5.0; + // } + // if (!pitch_angle_) { + // // throw BT::RuntimeError("Missing required input [pitch_angle]: ", pitch_angle_); + // RCLCPP_WARN( + // node_->get_logger(), "Missing required input [pitch_angle]. Using default value 0.0 degrees"); + // pitch_angle_.value() = 0.0; + // } + joint_cmd_pub_ = node_->create_publisher( + "/head_controller/joint_trajectory", 100); + joint_cmd_pub_->on_activate(); + + joint_state_sub_ = node_->create_subscription( + "/joint_states", 100, [this](const sensor_msgs::msg::JointState::SharedPtr msg) { + for (size_t i = 0; i < msg->name.size(); ++i) { + if (msg->name[i] == "head_1_joint") { // TODO: remove hardcoded joint name (TIAGo specific) + phase_ = msg->position[i]; + break; + } + } + }); +} + +void Pan::halt() +{ + joint_cmd_pub_->on_deactivate(); + node_->add_activation("attention_server"); +} + +double Pan::get_joint_yaw(double period, double range, double time, double phase) +{ + return std::clamp( + range * sin((2 * M_PI / period) * time + phase), -1.3, + 1.3); // TODO: remove hardcoded limits (TIAGo specific) +} + +BT::NodeStatus Pan::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + node_->remove_activation("attention_server"); + start_time_ = node_->now(); + phase_ = asin(phase_ / joint_range_); + joint_state_sub_ = nullptr; + } + + trajectory_msgs::msg::JointTrajectory command_msg; + auto elapsed = node_->now() - start_time_; + + double yaw = get_joint_yaw(period_, joint_range_, elapsed.seconds(), phase_); + + command_msg.joint_names = std::vector{ + "head_1_joint", "head_2_joint"}; // TODO: remove hardcoded joint names (TIAGo specific) + command_msg.points.resize(1); + command_msg.points[0].positions.resize(2); + command_msg.points[0].velocities.resize(2); + command_msg.points[0].accelerations.resize(2); + command_msg.points[0].positions[0] = std::clamp(yaw, -yaw_limit_, yaw_limit_); + command_msg.points[0].positions[1] = std::clamp(pitch_angle_, -pitch_limit_, pitch_limit_); + command_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.00); + joint_cmd_pub_->publish(command_msg); + rclcpp::spin_some(node_->get_node_base_interface()); + + return BT::NodeStatus::RUNNING; +} + +} // namespace head + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Pan"); +} diff --git a/motion/src/motion/navigation/navigate_through.cpp b/motion/src/motion/navigation/navigate_through.cpp new file mode 100644 index 0000000..972b429 --- /dev/null +++ b/motion/src/motion/navigation/navigate_through.cpp @@ -0,0 +1,204 @@ +// 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 "motion/navigation/navigate_through.hpp" + + +namespace navigation +{ + +NavigateThrough::NavigateThrough( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: motion::BtActionNode< + nav2_msgs::action::NavigateThroughPoses, rclcpp_cascade_lifecycle::CascadeLifecycleNode>( + xml_tag_name, action_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); +} + +void +NavigateThrough::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "NavigateThrough ticked"); + geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::TransformStamped map_to_goal, map_to_robot; + std::vector poses_to_goal; + int steps; + std::string tf_frame, xml_path, trajectory_style; + + getInput("tf_frame", tf_frame); + getInput("n_poses", steps); + getInput("style", trajectory_style); + + + if (tf_frame.length() > 0) { // There is a TF to go, ignore coordinates + RCLCPP_INFO(node_->get_logger(), "Transforming %s to %s", "map", tf_frame.c_str()); + try { + map_to_goal = tf_buffer_.lookupTransform("map", tf_frame, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + node_->get_logger(), "Could not transform %s to %s: %s", "map", tf_frame.c_str(), ex.what()); + setStatus(BT::NodeStatus::RUNNING); + } + + goal.pose.position.x = map_to_goal.transform.translation.x; + goal.pose.position.y = map_to_goal.transform.translation.y; + goal.pose.orientation.x = map_to_goal.transform.rotation.x; + goal.pose.orientation.y = map_to_goal.transform.rotation.y; + goal.pose.orientation.z = map_to_goal.transform.rotation.z; + goal.pose.orientation.w = map_to_goal.transform.rotation.w; + + } else { + getInput("x", map_to_goal.transform.translation.x); + getInput("y", map_to_goal.transform.translation.y); + goal.pose.position.x = map_to_goal.transform.translation.x; + goal.pose.position.y = map_to_goal.transform.translation.y; + goal.pose.position.z = 0.0; + + RCLCPP_INFO( + node_->get_logger(), "Sending coordinates"); + + goal.pose.orientation.x = 0.0; + goal.pose.orientation.y = 0.0; + goal.pose.orientation.z = 0.0; + goal.pose.orientation.w = 1.0; + } + + RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %f, y: %f", goal.pose.position.x, goal.pose.position.y); + + goal.header.frame_id = "map"; + + try { + map_to_robot = tf_buffer_.lookupTransform("map", "base_footprint", tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + node_->get_logger(), "Could not transform %s to %s: %s", "map", tf_frame.c_str(), ex.what()); + setStatus(BT::NodeStatus::RUNNING); + } + + poses_to_goal = generate_poses_to_goal(goal, map_to_goal, map_to_robot, steps, trajectory_style); + + RCLCPP_INFO( + node_->get_logger(), "Sending goal: x: %f, y: %f, qx: %f, qy: %f, qz: %f qw: %f. Frame: %s", + goal.pose.position.x, goal.pose.position.y, goal.pose.orientation.x, goal.pose.orientation.y, + goal.pose.orientation.z, goal.pose.orientation.w, goal.header.frame_id.c_str()); + + goal_.poses = poses_to_goal; +} + +BT::NodeStatus +NavigateThrough::on_success() +{ + RCLCPP_INFO(node_->get_logger(), "Navigation succeeded"); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus +NavigateThrough::on_aborted() +{ + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus +NavigateThrough::on_cancelled() +{ + return BT::NodeStatus::SUCCESS; +} + +std::vector +NavigateThrough::generate_poses_to_goal( + const geometry_msgs::msg::PoseStamped & goal, + geometry_msgs::msg::TransformStamped map_to_goal, + geometry_msgs::msg::TransformStamped map_to_robot, + int n_poses, + const std::string & style) +{ + std::vector poses; + + // Distance between robot and goal + double dx = map_to_goal.transform.translation.x - map_to_robot.transform.translation.x; + double dy = map_to_goal.transform.translation.y - map_to_robot.transform.translation.y; + double distance = std::sqrt(dx * dx + dy * dy); + + // Proportional radius to distance + double k = 0.3; + getInput("k_radius", k); + double radius = k * distance; + + RCLCPP_INFO( + node_->get_logger(), "Generating %d poses to goal with style %s", n_poses, style.c_str()); + + for (int i = 0; i < n_poses; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = goal.header.frame_id; + pose.header.stamp = node_->now(); + + // Fraction of the way (0.0 to 1.0) + double fraction = static_cast(i) / (n_poses - 1); + + if (style == "straight") { + // Linear interpolation + pose.pose.position.x = map_to_robot.transform.translation.x + + fraction * dx; + pose.pose.position.y = map_to_robot.transform.translation.y + + fraction * dy; + pose.pose.position.z = map_to_robot.transform.translation.z + + fraction * (map_to_goal.transform.translation.z - map_to_robot.transform.translation.z); + + } else if (style == "circular-l" || style == "circular-r") { + RCLCPP_INFO(node_->get_logger(), "Radius: %f", radius); + // Circular interpolation + double angle = fraction * M_PI; // From 0 to 180 degrees + double cx = (map_to_robot.transform.translation.x + map_to_goal.transform.translation.x) / 2.0; + double cy = (map_to_robot.transform.translation.y + map_to_goal.transform.translation.y) / 2.0; + + if (style == "circular-r") { + pose.pose.position.x = cx + radius * cos(angle); + pose.pose.position.y = cy + radius * sin(angle); + } else if (style == "circular-l") { + pose.pose.position.x = cx + radius * cos(angle); + pose.pose.position.y = cy - radius * sin(angle); + } + pose.pose.position.z = map_to_robot.transform.translation.z; + } + + // Same orientation as goal + pose.pose.orientation = goal.pose.orientation; + + RCLCPP_INFO( + node_->get_logger(), "Generated pose %d: x: %f, y: %f, qx: %f, qy: %f, qz: %f, qw: %f", + i + 1, pose.pose.position.x, pose.pose.position.y, pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z, pose.pose.orientation.w); + + poses.push_back(pose); + } + + return poses; +} + +} // namespace navigation + +#include "behaviortree_cpp_v3/bt_factory.h" + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "/navigate_through_poses", config); + }; + + factory.registerBuilder("NavigateThrough", builder); +} diff --git a/motion/src/motion/navigation/navigate_to.cpp b/motion/src/motion/navigation/navigate_to.cpp new file mode 100644 index 0000000..ee5f089 --- /dev/null +++ b/motion/src/motion/navigation/navigate_to.cpp @@ -0,0 +1,142 @@ +// 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 "motion/navigation/navigate_to.hpp" +#include + +namespace navigation +{ + +NavigateTo::NavigateTo( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: motion::BtActionNode( + xml_tag_name, action_name, conf) + // tf_buffer_(), + // tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + tf_buffer_ = std::make_unique(node_->get_clock()); + + tf_listener_ = std::make_unique(*tf_buffer_); +} + +void NavigateTo::on_tick() +{ + RCLCPP_INFO(node_->get_logger(), "NAVIGATE_TO"); + geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::TransformStamped map_to_goal; + std::string tf_frame, xml_path; + + getInput("tf_frame", tf_frame); + getInput("will_finish", will_finish_); + + double distance_tolerance = 0; + getInput("distance_tolerance", distance_tolerance); + + if (tf_frame.length() > 0) { // There is a TF to go, ignore coordinates + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: transforming map to %s", tf_frame.c_str()); + try { + map_to_goal = tf_buffer_->lookupTransform("map", tf_frame, tf2::TimePointZero); + // rclcpp::Time current_time = rclcpp::Clock(RCL_SYSTEM_TIME).now(); + // rclcpp::Time time_threshold = current_time - rclcpp::Duration::from_seconds(5.0); + // geometry_msgs::msg::TransformStamped map_to_goal = + // tf_buffer_->lookupTransform("map", tf_frame, tf2::TimePoint(std::chrono::seconds(time_threshold.nanoseconds() / 1000000000))); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN( + node_->get_logger(), "[NAVIGATE_TO]: could not transform map to %s: %s", tf_frame.c_str(), ex.what()); + setStatus(BT::NodeStatus::FAILURE); + return; + } + + goal.pose.position.x = map_to_goal.transform.translation.x; + goal.pose.position.y = map_to_goal.transform.translation.y; + goal.pose.position.z = 0.0; + goal.pose.orientation.x = map_to_goal.transform.rotation.x; + goal.pose.orientation.y = map_to_goal.transform.rotation.y; + goal.pose.orientation.z = map_to_goal.transform.rotation.z; + goal.pose.orientation.w = map_to_goal.transform.rotation.w; + + goal.header.frame_id = tf_frame; + } else { // No TF, use coordinates + + getInput("x", goal.pose.position.x); + getInput("y", goal.pose.position.y); + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: setting goal to x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); + + goal.pose.orientation.w = 1.0; + goal.pose.orientation.x = 0.0; + goal.pose.orientation.y = 0.0; + goal.pose.orientation.z = 0.0; + } + + goal.header.frame_id = "map"; + + RCLCPP_INFO( + node_->get_logger(), "[NAVIGATE_TO]: sending coordinates; frame: %s, x: %f, y: %f", goal.header.frame_id.c_str(),goal.pose.position.x, goal.pose.position.y); + + if (distance_tolerance != 0) { + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: setting distance tolerance to %.2f", distance_tolerance); + xml_path = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance); + goal_.behavior_tree = xml_path; + } + + goal_.pose = goal; +} + +BT::NodeStatus NavigateTo::on_success() +{ + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: navigation succeeded"); + if (will_finish_) { + return BT::NodeStatus::SUCCESS; + } + goal_updated_ = true; + on_tick(); + on_new_goal_received(); + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus NavigateTo::on_aborted() +{ + if (will_finish_) { + return BT::NodeStatus::FAILURE; + } + on_tick(); + on_new_goal_received(); + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus NavigateTo::on_cancelled() +{ + if (will_finish_) { + return BT::NodeStatus::SUCCESS; + } + on_tick(); + on_new_goal_received(); + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: navigation cancelled"); + return BT::NodeStatus::RUNNING; +} + +} // namespace navigation + +#include "behaviortree_cpp_v3/bt_factory.h" + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "navigate_to_pose", config); + }; + + factory.registerBuilder("NavigateTo", builder); +} diff --git a/motion/thirdparty.repos b/motion/thirdparty.repos new file mode 100644 index 0000000..ac49d1b --- /dev/null +++ b/motion/thirdparty.repos @@ -0,0 +1,13 @@ +repositories: + thirdparty/attention_system: + type: git + url: https://github.com/aaggj/attention_system + version: humble + thirdparty/cascade_lifecycle: + type: git + url: https://github.com/fmrico/cascade_lifecycle + version: main + thirdparty/navigation_system: + type: git + url: https://github.com/Juancams/navigation_system.git + version: main diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt new file mode 100644 index 0000000..494e552 --- /dev/null +++ b/perception/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.8) +project(perception_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(behaviortree_cpp_v3 REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(shape_msgs REQUIRED) +find_package(rclpy REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(perception_system REQUIRED) +find_package(OpenCV REQUIRED) +find_package(perception_system_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + ament_index_cpp + rclcpp + rclcpp_cascade_lifecycle + rclpy + behaviortree_cpp_v3 + tf2_ros + geometry_msgs + tf2_geometry_msgs + tf2 + rclcpp_action + perception_system + OpenCV + perception_system_interfaces + sensor_msgs + std_srvs +) + +include_directories(include) + +set(plugin_sources + src/perception/is_detected.cpp + src/perception/filter_entity.cpp + src/perception/is_pointing.cpp + src/perception/identify.cpp + src/perception/get_angle.cpp + src/perception/is_in_front.cpp + src/perception/is_in_view.cpp + src/perception/get_detection_from_tf.cpp + src/perception/get_detection_from_bb.cpp + src/perception/save_detection_in_bb.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(perception_test src/main_perception.cpp) +ament_target_dependencies(perception_test ${dependencies}) + +install(TARGETS + perception_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() diff --git a/perception/README.md b/perception/README.md new file mode 100644 index 0000000..c5a8c58 --- /dev/null +++ b/perception/README.md @@ -0,0 +1,2 @@ +# perception_bt_nodes +A collection of BT nodes for perception capabilities diff --git a/perception/bt_xml/align.xml b/perception/bt_xml/align.xml new file mode 100644 index 0000000..99b2b08 --- /dev/null +++ b/perception/bt_xml/align.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + If false (default), the Subtree has an isolated blackboard and needs port remapping + + + + diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml new file mode 100644 index 0000000..8fdec1e --- /dev/null +++ b/perception/bt_xml/detect_chair.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + Where to navigate + False in case it is navigating to follow something + + + + + + + + + + + diff --git a/perception/bt_xml/detect_person.xml b/perception/bt_xml/detect_person.xml new file mode 100644 index 0000000..6849945 --- /dev/null +++ b/perception/bt_xml/detect_person.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + Where to navigate + False in case it is navigating to follow something + + + + + + + + + + + diff --git a/perception/bt_xml/detect_person2.xml b/perception/bt_xml/detect_person2.xml new file mode 100644 index 0000000..4dac5f5 --- /dev/null +++ b/perception/bt_xml/detect_person2.xml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + Where to navigate + False in case it is navigating to follow something + + + + + + + + + + + diff --git a/perception/bt_xml/identify_person.xml b/perception/bt_xml/identify_person.xml new file mode 100644 index 0000000..550090c --- /dev/null +++ b/perception/bt_xml/identify_person.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/bt_xml/in_front.xml b/perception/bt_xml/in_front.xml new file mode 100644 index 0000000..a306aa7 --- /dev/null +++ b/perception/bt_xml/in_front.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/bt_xml/is_pointing.xml b/perception/bt_xml/is_pointing.xml new file mode 100644 index 0000000..2b0e60a --- /dev/null +++ b/perception/bt_xml/is_pointing.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + Where to navigate + False in case it is navigating to follow something + + + + + + + + + + + diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml new file mode 100644 index 0000000..055cfee --- /dev/null +++ b/perception/config/perception.yaml @@ -0,0 +1,12 @@ +perception_node: + ros__parameters: + use_sim_time: True + tf_frame_camera: head_front_camera_optical_frame # oak_rgb_camera_optical_frame / head_front_camera_optical_frame + bt_xml_file: detect_person.xml + plugins: + - is_in_front_bt_node + - is_detected_bt_node + - identify_bt_node + - spin_bt_node + - navigate_to_bt_node + - is_pointing_bt_node diff --git a/perception/include/perception/filter_entity.hpp b/perception/include/perception/filter_entity.hpp new file mode 100644 index 0000000..134ecd2 --- /dev/null +++ b/perception/include/perception/filter_entity.hpp @@ -0,0 +1,81 @@ +// 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 PERCEPTION__FILTER_ENTITY_HPP_ +#define PERCEPTION__FILTER_ENTITY_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#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 "perception_system/PerceptionUtils.hpp" +#include "sensor_msgs/msg/image.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("frame"), + BT::InputPort("lambda", "filtering parameter") + }); + } + +private: + 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); + + std::shared_ptr node_; + + std::string frame_; + double lambda_; + bool state_obs_initialized_ = false; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + + geometry_msgs::msg::TransformStamped filtered_entity_; + +}; + +} // namespace perception + +#endif // PERCEPTION__FILTER_ENTITY_HPP_ diff --git a/perception/include/perception/get_angle.hpp b/perception/include/perception/get_angle.hpp new file mode 100644 index 0000000..b2d1112 --- /dev/null +++ b/perception/include/perception/get_angle.hpp @@ -0,0 +1,71 @@ +// 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 PERCEPTION_GET_ANGLE_HPP_ +#define PERCEPTION_GET_ANGLE_HPP_ + +#include + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/twist.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/string.hpp" + +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +namespace perception +{ + +class GetAngle : public BT::ActionNodeBase +{ +public: + explicit GetAngle(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + void halt(); + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("tf_frame"), + BT::InputPort("base_frame"), + BT::InputPort("confidence"), + BT::OutputPort("yaw"), + BT::OutputPort("direction") + }); + } + +private: + std::shared_ptr node_; + + std::string base_frame_, tf_frame_; + std::string target_; + double confidence_; + +}; + +} // namespace perception + +#endif // PERCEPTION_GET_ANGLE_HPP_ diff --git a/perception/include/perception/get_detection_from_bb.hpp b/perception/include/perception/get_detection_from_bb.hpp new file mode 100644 index 0000000..cf2bd5e --- /dev/null +++ b/perception/include/perception/get_detection_from_bb.hpp @@ -0,0 +1,56 @@ +// 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 GET_FROM_BB__HPP +#define GET_FROM_BB__HPP + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "perception_system_interfaces/msg/detection.hpp" + + +namespace perception +{ + +class GetDetectionFromBB : public BT::ActionNodeBase +{ +public: + explicit GetDetectionFromBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + void halt(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("key"), + BT::OutputPort>("detection") + }); + } + +private: + + std::string key_; + +}; + +} // namespace perception + +#endif // GET_FROM_BB__HPP diff --git a/perception/include/perception/get_detection_from_tf.hpp b/perception/include/perception/get_detection_from_tf.hpp new file mode 100644 index 0000000..45ee35b --- /dev/null +++ b/perception/include/perception/get_detection_from_tf.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 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. + + +#ifndef GET_DETECTION_FROM_TF_HPP_ +#define GET_DETECTION_FROM_TF_HPP_ + +#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 +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/transform_datatypes.h" + +namespace perception +{ + +using namespace std::chrono_literals; +using std::placeholders::_1; + +class GetDetectionFromTF : public BT::ActionNodeBase +{ +public: + GetDetectionFromTF(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + void halt() override + {} + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("frame"), + BT::OutputPort>("detection") + }); + } + +private: + std::shared_ptr node_; + + std::string frame_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; +};; + +} // namespace perception + +#endif // GET_DETECTION_FROM_TF_HPP_ \ No newline at end of file diff --git a/perception/include/perception/identify.hpp b/perception/include/perception/identify.hpp new file mode 100644 index 0000000..3f6d86a --- /dev/null +++ b/perception/include/perception/identify.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 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. + + +#ifndef IDENTIFY_HPP_ +#define IDENTIFY_HPP_ + +#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 +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +namespace perception +{ + +using namespace std::chrono_literals; +using std::placeholders::_1; + +class Identify : public BT::ActionNodeBase +{ +public: + Identify(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + void halt() override + {} + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("entity_to_identify"), // Name of the entity to identify. Will be used as key in the blackboard + // A TF frame with the name of the entity will be published + BT::InputPort("confidence"), // Confidence threshold + BT::InputPort>("detection"), // Detection of the entity to identify. + // If present, it is stored in the blackboard for later use + }); + } + +private: + void save_detection_tf_to_bb(std::shared_ptr detection, std::string source_frame, std::string child_frame); + + std::shared_ptr node_; + + std::string entity_; + float confidence_; + + std::shared_ptr detection_; +};; + +} // namespace perception + +#endif // IDENTIFY_HPP_ \ No newline at end of file diff --git a/perception/include/perception/is_detected.hpp b/perception/include/perception/is_detected.hpp new file mode 100644 index 0000000..bbdb53c --- /dev/null +++ b/perception/include/perception/is_detected.hpp @@ -0,0 +1,72 @@ +// 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 PERCEPTION__ISDETECTED_HPP_ +#define PERCEPTION__ISDETECTED_HPP_ + +#include +#include + +#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 "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection_array.hpp" +#include "perception_system_interfaces/msg/detection.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +class IsDetected : public BT::ConditionNode +{ +public: + explicit IsDetected(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("max_entities"), // Max number of entities to consider + BT::InputPort("model"), // YOLO model (object or people) + BT::InputPort("interest"), // What to look for + BT::InputPort("confidence"), // Confidence threshold + BT::InputPort("order"), // How to sort the detections + BT::InputPort("max_depth"), // Max depth to consider + BT::InputPort("frame_name"), // Name of the frame to publish + BT::OutputPort>("best_detection"), // Best detected entity (first in the list) + BT::OutputPort("n_detections"), // Number of detections found + BT::OutputPort("frame") // Frame of the best detected entity (suffix: _1) + }); + } + +private: + std::shared_ptr node_; + + std::string interest_, order_, frame_name_; + double threshold_, max_depth_; + int max_entities_; + std::int64_t person_id_; + std::vector frames_; +}; + +} // namespace perception + +#endif // PERCEPTION__ISDETECTED_HPP_ diff --git a/perception/include/perception/is_in_front.hpp b/perception/include/perception/is_in_front.hpp new file mode 100644 index 0000000..d29daef --- /dev/null +++ b/perception/include/perception/is_in_front.hpp @@ -0,0 +1,71 @@ +// 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 PERCEPTION_IS_IN_FRONT_HPP_ +#define PERCEPTION_IS_IN_FRONT_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/twist.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/string.hpp" + +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/transform_datatypes.h" + +namespace perception +{ + +class IsInFront : public BT::ConditionNode +{ +public: + explicit IsInFront(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("entity_to_identify"), // Name of the tf frame of the entity to check if is in front + BT::OutputPort("direction") + }); + } + +private: + std::shared_ptr node_; + + std::string entity_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + +}; + +} // namespace perception + +#endif // PERCEPTION_IS_IN_FRONT_HPP_ diff --git a/perception/include/perception/is_in_view.hpp b/perception/include/perception/is_in_view.hpp new file mode 100644 index 0000000..a912105 --- /dev/null +++ b/perception/include/perception/is_in_view.hpp @@ -0,0 +1,75 @@ +// 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 PERCEPTION_IS_IN_VIEW_HPP_ +#define PERCEPTION_IS_IN_VIEW_HPP_ + +#include + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/twist.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/string.hpp" + +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/transform_datatypes.h" + + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +class IsInView : public BT::ConditionNode +{ +public: + explicit IsInView(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort>("detection") // Detection to check if is in view + }); + } + +private: + std::shared_ptr node_; + + std::shared_ptr detection_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + +}; + +} // namespace perception + +#endif // PERCEPTION_IS_IN_VIEW_HPP_ diff --git a/perception/include/perception/is_pointing.hpp b/perception/include/perception/is_pointing.hpp new file mode 100644 index 0000000..14d6d71 --- /dev/null +++ b/perception/include/perception/is_pointing.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__IS_POINTING_HPP_ +#define PERCEPTION__IS_POINTING_HPP_ + +#include +#include +#include +#include + + +#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 "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection_array.hpp" +#include "perception_system_interfaces/msg/detection.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + + +namespace perception +{ + +class IsPointing : public BT::ConditionNode +{ +public: + explicit IsPointing(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("low_pointing_limit"), + BT::InputPort("high_pointing_limit"), + BT::InputPort("threshold"), + BT::OutputPort>("detection"), + BT::OutputPort("pointing_direction"), + BT::OutputPort("output_frame") + }); + } + +private: + std::shared_ptr node_; + + std::string output_frame_ = "someone_pointing"; + int low_pointing_limit_, high_pointing_limit_; + int pointing_direction_; + float threshold_ = 0.6; + + }; + +} // namespace perception + +#endif // PERCEPTION__IS_POINTING_HPP_ diff --git a/perception/include/perception/save_detection_in_bb.hpp b/perception/include/perception/save_detection_in_bb.hpp new file mode 100644 index 0000000..db687dc --- /dev/null +++ b/perception/include/perception/save_detection_in_bb.hpp @@ -0,0 +1,55 @@ +// 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 SAVE_IN_BB__HPP +#define SAVE_IN_BB__HPP + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "perception_system_interfaces/msg/detection.hpp" + + +namespace perception +{ + +class SaveDetectionInBB : public BT::ActionNodeBase +{ +public: + explicit SaveDetectionInBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + + BT::NodeStatus tick(); + void halt(); + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("key"), + BT::InputPort>("detection") + }); + } + +private: + std::string key_; + perception_system_interfaces::msg::Detection detection_; +}; + +} // namespace perception + +#endif // SAVE_IN_BB__HPP diff --git a/perception/launch/perception.launch.py b/perception/launch/perception.launch.py new file mode 100644 index 0000000..74a3a02 --- /dev/null +++ b/perception/launch/perception.launch.py @@ -0,0 +1,50 @@ +# 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('perception_bt_nodes') + + params_file = os.path.join(pkg_dir, 'config', 'perception.yaml') + # print('params_file: ', params_file) + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['perception_node']['ros__parameters'] + # print(params) + + ld = LaunchDescription() + + nav_cmd = Node( + package='perception_bt_nodes', + executable='perception_test', + output='screen', + remappings=[ + ], + parameters=[{ + 'use_sim_time': True, + }, params] + ) + + ld.add_action(nav_cmd) + + return ld + diff --git a/perception/package.xml b/perception/package.xml new file mode 100644 index 0000000..b48fa65 --- /dev/null +++ b/perception/package.xml @@ -0,0 +1,39 @@ + + + + perception_bt_nodes + 0.0.0 + TODO: Package description + Samuele + rod + Juanca + Alberto + Apache License, Version 2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclcpp_cascade_lifecycle + behaviortree_cpp_v3 + yolo_msgs + moveit_msgs + shape_msgs + rclpy + tf2_ros + geometry_msgs + tf2_geometry_msgs + tf2 + rclcpp_action + perception_system + perception_system_interfaces + motion_bt_nodes + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/perception/src/main_perception.cpp b/perception/src/main_perception.cpp new file mode 100644 index 0000000..342190b --- /dev/null +++ b/perception/src/main_perception.cpp @@ -0,0 +1,76 @@ +// 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 +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("perception_node"); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + std::vector plugins; + std::string bt_xml_file; + node->declare_parameter("plugins", plugins); + node->declare_parameter("bt_xml_file", bt_xml_file); + // node->declare_parameter("tf_frame_camera", ""); + node->get_parameter("plugins", plugins); + node->get_parameter("bt_xml_file", bt_xml_file); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + for (const auto & plugin : plugins) { + RCLCPP_INFO(node->get_logger(), "Loading BT Node: [%s]", plugin.c_str()); + factory.registerFromPlugin(loader.getOSName(plugin)); + } + + std::string pkgpath = ament_index_cpp::get_package_share_directory("perception_bt_nodes"); + std::string xml_file = pkgpath + "/bt_xml/" + bt_xml_file; + + RCLCPP_INFO(node->get_logger(), "Loading BT: [%s]", xml_file.c_str()); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", node); + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + + RCLCPP_INFO(node->get_logger(), "Starting behavior from BT"); + + auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); + + rclcpp::Rate rate(15); + + bool finish = false; + while (!finish && rclcpp::ok()) { + finish = tree.tickRoot() != BT::NodeStatus::RUNNING; + rclcpp::spin_some(node->get_node_base_interface()); + rate.sleep(); + } + + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + + rclcpp::shutdown(); + return 0; +} diff --git a/perception/src/perception/filter_entity.cpp b/perception/src/perception/filter_entity.cpp new file mode 100644 index 0000000..671127b --- /dev/null +++ b/perception/src/perception/filter_entity.cpp @@ -0,0 +1,104 @@ +// 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.hpp" + + + +namespace perception +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; + +FilterEntity::FilterEntity(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(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + getInput("lambda", lambda_); + tf_broadcaster_ = std::make_unique(node_); + RCLCPP_INFO(node_->get_logger(), "FilterEntity initialized"); +} + +void FilterEntity::halt() {RCLCPP_INFO(node_->get_logger(), "FilterEntity halted");} + +geometry_msgs::msg::TransformStamped FilterEntity::update_state_observer( + const geometry_msgs::msg::TransformStamped & entity) +{ + filtered_entity_.transform.translation.x = + filtered_entity_.transform.translation.x + + lambda_ * (entity.transform.translation.x - filtered_entity_.transform.translation.x); + filtered_entity_.transform.translation.y = + filtered_entity_.transform.translation.y + + lambda_ * (entity.transform.translation.y - filtered_entity_.transform.translation.y); + filtered_entity_.transform.translation.z = + filtered_entity_.transform.translation.z + + lambda_ * (entity.transform.translation.z - filtered_entity_.transform.translation.z); + filtered_entity_.header.stamp = entity.header.stamp; + return filtered_entity_; +} +geometry_msgs::msg::TransformStamped FilterEntity::initialize_state_observer( + const geometry_msgs::msg::TransformStamped & entity) +{ + filtered_entity_ = entity; + filtered_entity_.child_frame_id = entity.child_frame_id + "_filtered"; + return filtered_entity_; +} + +BT::NodeStatus FilterEntity::tick() +{ + getInput("frame", frame_); + RCLCPP_INFO(node_->get_logger(), "Filtering frame %s", frame_.c_str()); + + geometry_msgs::msg::TransformStamped entity_transform_now_msg; + + try { + entity_transform_now_msg = tf_buffer_->lookupTransform("map", frame_, tf2::TimePointZero); + RCLCPP_INFO( + node_->get_logger(), "Transform %s to %s: %f %f %f", frame_.c_str(), "map", + 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(), "map", ex.what()); + RCLCPP_INFO(node_->get_logger(), "Cannot transform"); + + return BT::NodeStatus::SUCCESS; // Never return FAILURE, the filter will try again next tick + } + + 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 = update_state_observer(entity_transform_now_msg); + // 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("FilterEntity"); +} diff --git a/perception/src/perception/get_angle.cpp b/perception/src/perception/get_angle.cpp new file mode 100644 index 0000000..0830e91 --- /dev/null +++ b/perception/src/perception/get_angle.cpp @@ -0,0 +1,86 @@ +// 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 "perception/get_angle.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +GetAngle::GetAngle(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("base_frame", base_frame_); + getInput("tf_frame", tf_frame_); + getInput("target", target_); + getInput("conficende", confidence_); + getInput("what", what); + + if (what == "person") { + node_->add_activation("perception_system/perception_people_detection"); + } else if (what == "object") { + node_->add_activation("perception_system/perception_object_detection"); + } else { + RCLCPP_ERROR(node_->get_logger(), "[IsDetected] Unknown what: %s. Activating generic", what.c_str()); + node_->add_activation("perception_system/perception_object_detection"); + } +} +BT::NodeStatus GetAngle::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "GetAngle ticked"); + rclcpp::spin_some(node_->get_node_base_interface()); + + std::vector detections; + perception_system_interfaces::msg::Detection detection; + + config().blackboard->get(target_, detection); + + + detections = pl::getInstance(node_)->get_by_features(detection, confidence_); + + + if (detections.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections found"); + return BT::NodeStatus::FAILURE; + } + + // double dx = detection.center3d.position.x; + // double dy = detection.center3d.position.y; + // double yaw = atan2(dx, -dy); + double yaw = atan2(detection.center3d.position.y, detection.center3d.position.x); + + setOutput("yaw", yaw); + if (yaw > 0) { + setOutput("direction", 1); + } else { + setOutput("direction", -1); + } + + return BT::NodeStatus::SUCCESS; +} + +void GetAngle::halt() +{ + RCLCPP_DEBUG(node_->get_logger(), "GetAngle halted"); +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("GetAngle"); +} diff --git a/perception/src/perception/get_detection_from_bb.cpp b/perception/src/perception/get_detection_from_bb.cpp new file mode 100644 index 0000000..17a7509 --- /dev/null +++ b/perception/src/perception/get_detection_from_bb.cpp @@ -0,0 +1,45 @@ +// 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 "perception/get_detection_from_bb.hpp" + +namespace perception +{ + + +GetDetectionFromBB::GetDetectionFromBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + getInput("key", key_); +} +BT::NodeStatus GetDetectionFromBB::tick() +{ + + std::shared_ptr detection_ptr; + + config().blackboard->get(key_, detection_ptr); + RCLCPP_INFO(rclcpp::get_logger("bt_node"), "Detection read from blackboard"); + setOutput("detection", detection_ptr); + return BT::NodeStatus::SUCCESS; +} + +void GetDetectionFromBB::halt() +{ +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("GetDetectionFromBB"); +} diff --git a/perception/src/perception/get_detection_from_tf.cpp b/perception/src/perception/get_detection_from_tf.cpp new file mode 100644 index 0000000..367a5c7 --- /dev/null +++ b/perception/src/perception/get_detection_from_tf.cpp @@ -0,0 +1,69 @@ +// Copyright 2024 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. + + +#include "perception/get_detection_from_tf.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +GetDetectionFromTF::GetDetectionFromTF( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + getInput("frame", frame_); +} + +BT::NodeStatus +GetDetectionFromTF::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "GET_DETECTION_FROM_TF"); + rclcpp::spin_some(node_->get_node_base_interface()); + + geometry_msgs::msg::TransformStamped base2entity_msg; + try { + base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(node_->get_logger(), "Could not transform %s to base_link: %s", frame_.c_str(), ex.what()); + return BT::NodeStatus::FAILURE; + } + + pl::getInstance(node_)->update(30); + auto detections = pl::getInstance(node_)->get_detection_at(base2entity_msg); + + if (detections.empty()) { + RCLCPP_INFO(node_->get_logger(), "No detection found at (%.2f, %.2f, %.2f) from base_link", + base2entity_msg.transform.translation.x, + base2entity_msg.transform.translation.y, + base2entity_msg.transform.translation.z); + return BT::NodeStatus::FAILURE; + } + + setOutput("detection", std::make_shared(detections[0])); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("GetDetectionFromTF"); +} + diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp new file mode 100644 index 0000000..5e0f181 --- /dev/null +++ b/perception/src/perception/identify.cpp @@ -0,0 +1,104 @@ +// Copyright 2024 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. + + +#include "perception/identify.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +Identify::Identify( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf), + entity_(""), + confidence_(0.2) +{ + config().blackboard->get("node", node_); + + getInput("entity_to_identify", entity_); + RCLCPP_INFO(node_->get_logger(), "Identifying %s", entity_.c_str()); + + if (!getInput("confidence", confidence_)) { + RCLCPP_WARN(node_->get_logger(), "No confidence provided. Using default value %f", confidence_); + } else { + RCLCPP_DEBUG(node_->get_logger(), "Confidence threshold: %.2f", confidence_); + } + +} + +BT::NodeStatus +Identify::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IDENTIFY"); + rclcpp::spin_some(node_->get_node_base_interface()); + std::vector detections; + + if (!getInput("detection", detection_)) { + RCLCPP_ERROR_ONCE(node_->get_logger(), "[IDENTIFY]: No detection at input"); + return BT::NodeStatus::FAILURE; + } + + pl::getInstance(node_)->update(30); + pl::getInstance(node_)->set_interest("person", true); + // detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); + detections = pl::getInstance(node_)->get_by_type(detection_->type); //Until getting by features works + + // save_detection_tf_to_bb(detection_, "map", entity_); + + if (detections.empty()) { + RCLCPP_WARN_ONCE(node_->get_logger(), "[IDENTIFY]: Perception system did not identify %s", entity_.c_str()); + return BT::NodeStatus::FAILURE; + } + + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return a.score > b.score; + // }); + + // Publish the detection + RCLCPP_DEBUG(node_->get_logger(), "[IDENTIFY]: Detected %s with confidence %.2f", entity_.c_str(), detections[0].score); + pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); + RCLCPP_DEBUG(node_->get_logger(), "[IDENTIFY]: Published TF for %s", entity_.c_str()); + detections.clear(); + return BT::NodeStatus::SUCCESS; +} + +void +Identify::save_detection_tf_to_bb(std::shared_ptr detection, std::string source_frame, std::string child_frame) +{ + std::shared_ptr map2person_msg_ptr = std::make_shared(); + + map2person_msg_ptr->header.stamp = detection->header.stamp; + map2person_msg_ptr->header.frame_id = source_frame; + map2person_msg_ptr->child_frame_id = child_frame; + map2person_msg_ptr->transform.translation.x = detection->center3d.position.x; + map2person_msg_ptr->transform.translation.y = detection->center3d.position.y; + map2person_msg_ptr->transform.translation.z = detection->center3d.position.z; + map2person_msg_ptr->transform.rotation.x = 0.0; + map2person_msg_ptr->transform.rotation.y = 0.0; + map2person_msg_ptr->transform.rotation.z = 0.0; + map2person_msg_ptr->transform.rotation.w = 1.0; + + config().blackboard->set("last_map2person", map2person_msg_ptr); +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Identify"); +} + diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp new file mode 100644 index 0000000..ca87097 --- /dev/null +++ b/perception/src/perception/is_detected.cpp @@ -0,0 +1,146 @@ +// 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 "perception/is_detected.hpp" + +namespace perception +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; + +IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ConditionNode(xml_tag_name, conf), + max_depth_(std::numeric_limits::max()), + max_entities_(1) +{ + config().blackboard->get("node", node_); + + std::string model; + getInput("model", model); + + if (model == "people") { + RCLCPP_DEBUG(node_->get_logger(), "Activating perception_people_detection"); + node_->add_activation("perception_system/perception_people_detection"); + } else if (model == "object") { + RCLCPP_DEBUG(node_->get_logger(), "Activating perception_object_detection"); + node_->add_activation("perception_system/perception_object_detection"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Unknown model: %s. Activating generic", model.c_str()); + node_->add_activation("perception_system/perception_object_detection"); + } + + getInput("interest", interest_); + getInput("confidence", threshold_); + getInput("max_entities", max_entities_); + getInput("order", order_); + getInput("max_depth", max_depth_); + getInput("frame_name", frame_name_); + + RCLCPP_DEBUG(node_->get_logger(), "Interest: %s", interest_.c_str()); + + frames_.clear(); +} + +BT::NodeStatus IsDetected::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IS_DETECTED"); + rclcpp::spin_some(node_->get_node_base_interface()); + + pl::getInstance(node_)->set_interest(interest_, true); + pl::getInstance(node_)->update(35); + + auto detections = pl::getInstance(node_)->get_by_type(interest_); + + if (detections.empty()) { + RCLCPP_DEBUG(node_->get_logger(), "No %s detections", interest_.c_str()); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_DEBUG(node_->get_logger(), "Processing %ld detections...", detections.size()); + + if (order_ == "color") { + // sorted by the distance to the color person we should sort it by distance and also by left to right or right to left + // RCLCPP_DEBUG(node_->get_logger(), "Sorting detections by color"); + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return perception_system::diffIDs(this->person_id_, a.color_person) < + // perception_system::diffIDs(this->person_id_, b.color_person); + // }); + } else if (order_ == "depth") { + RCLCPP_DEBUG(node_->get_logger(), "Sorting detections by depth"); + std::sort( + detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + return a.center3d.position.z < b.center3d.position.z; + }); + } + // auto pub = node_->create_publisher( + // "/object_detected", 10); + + // pub->publish(detections[0].image); + + + RCLCPP_DEBUG(node_->get_logger(), "Max Depth: %f", max_depth_); + RCLCPP_DEBUG(node_->get_logger(), "Threshold: %f", threshold_); + auto entity_counter = 0; + for (auto it = detections.begin(); it != detections.end() && entity_counter < max_entities_; ) { + auto const & detection = *it; + if (detection.score <= threshold_ || detection.center3d.position.z > max_depth_) { + RCLCPP_DEBUG( + node_->get_logger(), "Removing detection %s", detection.class_name.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Score: %f", detection.score); + RCLCPP_DEBUG(node_->get_logger(), "Depth: %f", detection.center3d.position.z); + it = detections.erase(it); + + } else { + std::string tf_name; + + if (frame_name_ == "") { + tf_name = detection.class_name; + } else { + tf_name = frame_name_; + } + tf_name = tf_name + "_" + std::to_string(entity_counter + 1); + frames_.push_back(tf_name); + if (pl::getInstance(node_)->publishTF_EKF(detection, tf_name, true) == -1) { + return BT::NodeStatus::FAILURE; + } + RCLCPP_INFO(node_->get_logger(), "Publishing TF %s", tf_name.c_str()); + ++it; + ++entity_counter; + } + } + + RCLCPP_DEBUG(node_->get_logger(), "Detections sorted and filtered"); + if (frames_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections after filtering"); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_INFO(node_->get_logger(), "A %s has been found with the provided conditions", detections[0].class_name.c_str()); + setOutput("best_detection", std::make_shared(detections[0])); + setOutput("n_dectections", static_cast(detections.size()) + 1); + setOutput("frame", frames_[0]); // TF frame of the best detected entity + + // frames_.clear(); + + RCLCPP_DEBUG(node_->get_logger(), "Detections published"); + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsDetected"); +} diff --git a/perception/src/perception/is_in_front.cpp b/perception/src/perception/is_in_front.cpp new file mode 100644 index 0000000..078c1e4 --- /dev/null +++ b/perception/src/perception/is_in_front.cpp @@ -0,0 +1,76 @@ +// 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 "perception/is_in_front.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +IsInFront::IsInFront(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ConditionNode(xml_tag_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + + getInput("entity_to_identify", entity_); +} +BT::NodeStatus IsInFront::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IS_IN_FRONT"); + rclcpp::spin_some(node_->get_node_base_interface()); + + geometry_msgs::msg::TransformStamped base2entity_msg; + tf2::Stamped base2entity; + + base2entity_msg = tf_buffer_.lookupTransform("base_link", entity_, tf2::TimePointZero); + tf2::fromMsg(base2entity_msg, base2entity); + + // double yaw = tf2::getYaw(base2entity.getRotation()); + double yaw = std::atan2(base2entity.getOrigin().y(), base2entity.getOrigin().x()); + + yaw = yaw * 180.0 / M_PI; + + RCLCPP_DEBUG(node_->get_logger(), "Detection at (%.2f, %.2f, %.2f) from base_footprint. YAW: %.2f degrees", + base2entity.getOrigin().x(), + base2entity.getOrigin().y(), + base2entity.getOrigin().z(), + yaw); + + if (std::abs((yaw)) > 5.0) { // If the angle is greater than 5 degrees, the detection is not in front + if (yaw > 0) { + RCLCPP_DEBUG(node_->get_logger(), "Detection is at %.2f degrees to the left", yaw); + setOutput("direction", 1.0); // Left + } else { + RCLCPP_DEBUG(node_->get_logger(), "Detection is at %.2f degrees to the right", yaw); + setOutput("direction", -1.0); // Right + } + return BT::NodeStatus::FAILURE; + } + + // The detection is in front. Publish the detection + RCLCPP_DEBUG(node_->get_logger(), "Detection is in front!"); + setOutput("direction", 0.0); + return BT::NodeStatus::SUCCESS; + + +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsInFront"); +} diff --git a/perception/src/perception/is_in_view.cpp b/perception/src/perception/is_in_view.cpp new file mode 100644 index 0000000..d7ca676 --- /dev/null +++ b/perception/src/perception/is_in_view.cpp @@ -0,0 +1,57 @@ +// 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 "perception/is_in_view.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +IsInView::IsInView(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ConditionNode(xml_tag_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + + getInput("detection", detection_); +} +BT::NodeStatus IsInView::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IS_IN_VIEW"); + rclcpp::spin_some(node_->get_node_base_interface()); + + pl::getInstance(node_)->update(30); + // auto detections = pl::getInstance(node_)->get_by_id(detection_->unique_i d); + auto detections = pl::getInstance(node_)->get_by_type(detection_->type); //Until getting by ID works + RCLCPP_DEBUG(node_->get_logger(), "Detection obtained by ID"); + + if (detections.empty()) { + RCLCPP_INFO(node_->get_logger(), "Detection NOT in view: FAILURE"); + return BT::NodeStatus::FAILURE; + } else if (detections[0].score < 0.5) { + RCLCPP_INFO(node_->get_logger(), "Detection in view but score is too low"); + return BT::NodeStatus::FAILURE; + } + RCLCPP_DEBUG(node_->get_logger(), "Detection IN view"); + return BT::NodeStatus::SUCCESS; + +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsInView"); +} diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp new file mode 100644 index 0000000..658e300 --- /dev/null +++ b/perception/src/perception/is_pointing.cpp @@ -0,0 +1,122 @@ +// 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/is_pointing.hpp" + + +namespace perception +{ + +using namespace std::chrono_literals; +using namespace std::placeholders; + +using pl = perception_system::PerceptionListener; + +IsPointing::IsPointing(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ConditionNode(xml_tag_name, conf) +{ + config().blackboard->get("node", node_); + + getInput("low_pointing_limit", low_pointing_limit_); + getInput("high_pointing_limit", high_pointing_limit_); + getInput("threshold", threshold_); + + RCLCPP_INFO(node_->get_logger(), "Activating perception_people_detection"); + node_->add_activation("perception_system/perception_people_detection"); +} + +BT::NodeStatus IsPointing::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IS_POINTING"); + rclcpp::spin_some(node_->get_node_base_interface()); + if (status() == BT::NodeStatus::IDLE) { + RCLCPP_DEBUG(node_->get_logger(), "IsPointing ticked while IDLE"); + } + + pl::getInstance(node_)->set_interest("person", true); + pl::getInstance(node_)->update(35); + + auto detections = pl::getInstance(node_)->get_by_type("person"); + + if (detections.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections"); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_INFO(node_->get_logger(), "Processing %ld detections...", detections.size()); + + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return perception_system::diffIDs(this->person_id_, a.color_person) < + // perception_system::diffIDs(this->person_id_, b.color_person); + // }); + + // Remove detections with low confidence + for (auto it = detections.begin(); it != detections.end(); ) { + auto const & detection = *it; + if (detection.score <= threshold_) { + RCLCPP_INFO( + node_->get_logger(), "Removing detection %s due to low confidence (%f)", + detection.class_name.c_str(), detection.score); + it = detections.erase(it); // If low confidence, remove and go to next + } else { + ++it; // If kept, just go to the next + } + } + + if (detections.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections above confidence threshold"); + return BT::NodeStatus::FAILURE; + } + + auto best_detection = std::make_shared(detections[0]); + + RCLCPP_INFO(node_->get_logger(), "Best detection pointing direction: %d", + best_detection->pointing_direction); + + int direction = -1; + for (int i = low_pointing_limit_; i <= high_pointing_limit_; i++) { + RCLCPP_INFO( + node_->get_logger(), "Pointing direction %d", + best_detection->pointing_direction); + if (best_detection->pointing_direction == i) { + direction = i; + RCLCPP_INFO(node_->get_logger(), "I like the direction the person is pointing at: %d", direction); + setOutput("pointing_direction", direction); + setOutput("detection", best_detection); + if (pl::getInstance(node_)->publishTF_EKF(*best_detection, output_frame_, true) == -1) { + return BT::NodeStatus::FAILURE; + } + break; + } + } + + if (direction == -1) { // No match + RCLCPP_ERROR(node_->get_logger(), "Invalid pointing direction"); + return BT::NodeStatus::FAILURE; + } + + // If someones is pointing to a valid direction, return SUCCESS and populate remaining output ports + setOutput("output_frame", output_frame_); + + RCLCPP_INFO(node_->get_logger(), "Someone (%s) is pointing to a valid direction: %d", output_frame_.c_str(), direction); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsPointing"); +} diff --git a/perception/src/perception/save_detection_in_bb.cpp b/perception/src/perception/save_detection_in_bb.cpp new file mode 100644 index 0000000..ebcf55a --- /dev/null +++ b/perception/src/perception/save_detection_in_bb.cpp @@ -0,0 +1,44 @@ +// 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 "perception/save_detection_in_bb.hpp" + +namespace perception +{ + + +SaveDetectionInBB::SaveDetectionInBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + + getInput("key", key_); + getInput("detection", detection_); + +} +BT::NodeStatus SaveDetectionInBB::tick() +{ + config().blackboard->set("detection", std::make_shared(detection_)); + RCLCPP_INFO(rclcpp::get_logger("bt_node"), "Detection saved in blackboard"); + return BT::NodeStatus::SUCCESS; +} + +void SaveDetectionInBB::halt() +{ +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("SaveDetectionInBB"); +} diff --git a/perception/thirdparty.repos b/perception/thirdparty.repos new file mode 100644 index 0000000..b32b169 --- /dev/null +++ b/perception/thirdparty.repos @@ -0,0 +1,9 @@ +repositories: + thirdparty/perception_system: + type: git + url: https://github.com/jmguerreroh/perception_system + version: main + thirdparty/cascade_lifecycle: + type: git + url: https://github.com/fmrico/cascade_lifecycle + version: main