From 26280fc58fabcd0167d60a8563183d9d98af8502 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 13 Nov 2024 16:29:40 +0100 Subject: [PATCH 01/60] hri nodes --- hri/CMakeLists.txt | 93 ++++++++ hri/bt_xml/listen_and_repeat.xml | 22 ++ hri/bt_xml/query.xml | 28 +++ hri/bt_xml/speak.xml | 15 ++ hri/config/grammar.txt | 25 ++ hri/config/hri.yaml | 12 + hri/config/prompt.txt | 4 + hri/include/ctrl_support/BTActionNode.hpp | 275 ++++++++++++++++++++++ hri/include/hri/dialog/Listen.hpp | 58 +++++ hri/include/hri/dialog/Query.hpp | 73 ++++++ hri/include/hri/dialog/Speak.hpp | 72 ++++++ hri/launch/hri.launch.py | 50 ++++ hri/launch/hri_dependencies.launch.py | 91 +++++++ hri/package.xml | 28 +++ hri/src/hri/dialog/Listen.cpp | 67 ++++++ hri/src/hri/dialog/Query.cpp | 155 ++++++++++++ hri/src/hri/dialog/Speak.cpp | 116 +++++++++ hri/src/hri/main_hri.cpp | 73 ++++++ hri/thirdparty.repos | 19 ++ 19 files changed, 1276 insertions(+) create mode 100644 hri/CMakeLists.txt create mode 100644 hri/bt_xml/listen_and_repeat.xml create mode 100644 hri/bt_xml/query.xml create mode 100644 hri/bt_xml/speak.xml create mode 100644 hri/config/grammar.txt create mode 100644 hri/config/hri.yaml create mode 100644 hri/config/prompt.txt create mode 100644 hri/include/ctrl_support/BTActionNode.hpp create mode 100644 hri/include/hri/dialog/Listen.hpp create mode 100644 hri/include/hri/dialog/Query.hpp create mode 100644 hri/include/hri/dialog/Speak.hpp create mode 100644 hri/launch/hri.launch.py create mode 100644 hri/launch/hri_dependencies.launch.py create mode 100644 hri/package.xml create mode 100644 hri/src/hri/dialog/Listen.cpp create mode 100644 hri/src/hri/dialog/Query.cpp create mode 100644 hri/src/hri/dialog/Speak.cpp create mode 100644 hri/src/hri/main_hri.cpp create mode 100644 hri/thirdparty.repos diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt new file mode 100644 index 0000000..6c090e3 --- /dev/null +++ b/hri/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.8) +project(hri) + +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(query_bt_node SHARED src/hri/dialog/Query.cpp) + +list(APPEND plugin_libs + listen_bt_node + speak_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/hri/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}) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_package() 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..f520208 --- /dev/null +++ b/hri/config/hri.yaml @@ -0,0 +1,12 @@ +hri_node: + ros__parameters: + use_sim_time: False + bt_xml_file: query.xml + prompt_file: prompt.txt + grammar_file: grammar.txt + placeholder: "[]" + plugins: + - speak_bt_node + - listen_bt_node + - query_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/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..e6ed9d0 --- /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') + + 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', + 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..928fdd5 --- /dev/null +++ b/hri/launch/hri_dependencies.launch.py @@ -0,0 +1,91 @@ +# 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'} + .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': 'cuda'}] + ) + + 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..98f5b16 --- /dev/null +++ b/hri/package.xml @@ -0,0 +1,28 @@ + + + + hri + 0.0.0 + TODO: Package description + Samuele + 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/Listen.cpp b/hri/src/hri/dialog/Listen.cpp new file mode 100644 index 0000000..baf0741 --- /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..59bc370 --- /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"); + 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..29100f5 --- /dev/null +++ b/hri/src/hri/dialog/Speak.cpp @@ -0,0 +1,116 @@ +// 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_); + + 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_DEBUG(node_->get_logger(), "Speak ticked"); + + + 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/hri/main_hri.cpp b/hri/src/hri/main_hri.cpp new file mode 100644 index 0000000..8885cb1 --- /dev/null +++ b/hri/src/hri/main_hri.cpp @@ -0,0 +1,73 @@ +// 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"); + 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::shutdown(); + return 0; +} diff --git a/hri/thirdparty.repos b/hri/thirdparty.repos new file mode 100644 index 0000000..9c190f3 --- /dev/null +++ b/hri/thirdparty.repos @@ -0,0 +1,19 @@ +repositories: + thirdparty/cascade_lifecycle: + type: git + 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 + + \ No newline at end of file From 26f5a9db64d3ee006d7131c8a28a4dda17554c8f Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 14 Nov 2024 15:33:26 +0100 Subject: [PATCH 02/60] DialogConfirmation refined --- hri/CMakeLists.txt | 4 +- hri/include/hri/dialog/DialogConfirmation.hpp | 54 +++++++++++++ hri/package.xml | 3 +- hri/src/hri/dialog/DialogConfirmation.cpp | 80 +++++++++++++++++++ 4 files changed, 139 insertions(+), 2 deletions(-) create mode 100644 hri/include/hri/dialog/DialogConfirmation.hpp create mode 100644 hri/src/hri/dialog/DialogConfirmation.cpp diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt index 6c090e3..54c7d8e 100644 --- a/hri/CMakeLists.txt +++ b/hri/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(hri) +project(hri_bt_nodes) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -49,11 +49,13 @@ 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/Speak.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 ) diff --git a/hri/include/hri/dialog/DialogConfirmation.hpp b/hri/include/hri/dialog/DialogConfirmation.hpp new file mode 100644 index 0000000..9ce9822 --- /dev/null +++ b/hri/include/hri/dialog/DialogConfirmation.hpp @@ -0,0 +1,54 @@ +// 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 "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "hri/dialog/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 DialogConfirmation + : public dialog::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({});} + +private: + rclcpp::Publisher::SharedPtr speech_start_publisher_; + + const int START_LISTENING_{0}; +}; + +} // namespace dialog + +#endif // HRI__DIALOGCONFIRMATION_HPP_ diff --git a/hri/package.xml b/hri/package.xml index 98f5b16..83f59ed 100644 --- a/hri/package.xml +++ b/hri/package.xml @@ -1,10 +1,11 @@ - hri + hri_bt_nodes 0.0.0 TODO: Package description Samuele + rod Apache License, Version 2.0 ament_cmake diff --git a/hri/src/hri/dialog/DialogConfirmation.cpp b/hri/src/hri/dialog/DialogConfirmation.cpp new file mode 100644 index 0000000..27b5137 --- /dev/null +++ b/hri/src/hri/dialog/DialogConfirmation.cpp @@ -0,0 +1,80 @@ +// 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/DialogConfirmation.hpp" + +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "hri/dialog/DialogConfirmation.hpp" +#include "std_msgs/msg/int8.hpp" +#include "whisper_msgs/action/stt.hpp" + +namespace dialog +{ + +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) +: dialog::BtActionNode( + xml_tag_name, action_name, conf) +{ + speech_start_publisher_ = node_->create_publisher("dialog_action", 10); +} + +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);}); + if (result_.result->transcription.text.find("yes") != std::string::npos) { + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::FAILURE; + } +} + +} // 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("DialogConfirmation", builder); +} From 5421d5f5a02429fe34d0d2caccdb296e137dd78a Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 14 Nov 2024 15:36:01 +0100 Subject: [PATCH 03/60] Navigation bt nodes refined + one added --- motion/CMakeLists.txt | 98 +++++++ motion/README.md | 2 + motion/include/ctrl_support/BTActionNode.hpp | 275 ++++++++++++++++++ motion/include/motion/base/Rotate.hpp | 66 +++++ motion/include/motion/head/Pan.hpp | 73 +++++ .../motion/navigation/NavigateThrough.hpp | 79 +++++ .../include/motion/navigation/NavigateTo.hpp | 96 ++++++ motion/include/motion/navigation/utils.hpp | 120 ++++++++ motion/package.xml | 33 +++ motion/src/motion/base/Rotate.cpp | 69 +++++ motion/src/motion/head/Pan.cpp | 113 +++++++ .../src/motion/navigation/NavigateThrough.cpp | 147 ++++++++++ motion/src/motion/navigation/NavigateTo.cpp | 149 ++++++++++ motion/thirdparty.repos | 13 + 14 files changed, 1333 insertions(+) create mode 100644 motion/CMakeLists.txt create mode 100644 motion/README.md create mode 100644 motion/include/ctrl_support/BTActionNode.hpp create mode 100644 motion/include/motion/base/Rotate.hpp create mode 100644 motion/include/motion/head/Pan.hpp create mode 100644 motion/include/motion/navigation/NavigateThrough.hpp create mode 100644 motion/include/motion/navigation/NavigateTo.hpp create mode 100644 motion/include/motion/navigation/utils.hpp create mode 100644 motion/package.xml create mode 100644 motion/src/motion/base/Rotate.cpp create mode 100644 motion/src/motion/head/Pan.cpp create mode 100644 motion/src/motion/navigation/NavigateThrough.cpp create mode 100644 motion/src/motion/navigation/NavigateTo.cpp create mode 100644 motion/thirdparty.repos diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt new file mode 100644 index 0000000..87656ee --- /dev/null +++ b/motion/CMakeLists.txt @@ -0,0 +1,98 @@ +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(nav2_costmap_2d REQUIRED) +# find_package(pluginlib REQUIRED) +find_package(navigation_system_interfaces REQUIRED) +# find_package(slam_toolbox REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(trajectory_msgs REQUIRED) +# find_package(lifecycle_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 + # nav2_costmap_2d + # pluginlib + navigation_system_interfaces + # slam_toolbox + ament_index_cpp + trajectory_msgs + # lifecycle_msgs + sensor_msgs +) + +include_directories(include) + +add_library(navigate_to_bt_node SHARED src/motion/navigation/NavigateTo.cpp) +add_library(navigate_through_bt_node SHARED src/motion/navigation/NavigateThrough.cpp) +add_library(rotate_bt_node SHARED src/motion/base/Rotate.cpp) +add_library(pan_bt_node SHARED src/motion/head/Pan.cpp) + +list(APPEND + plugin_libs + navigate_to_bt_node + navigate_through_bt_node + rotate_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() + +install(TARGETS + ${plugin_libs} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) +# install(FILES clear_people_layer.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_libraries(clear_people_layer) +ament_export_dependencies(${dependencies}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +# pluginlib_export_plugin_description_file(nav2_costmap_2d clear_people_layer.xml) + +ament_package() diff --git a/motion/README.md b/motion/README.md new file mode 100644 index 0000000..5952267 --- /dev/null +++ b/motion/README.md @@ -0,0 +1,2 @@ +# motion_bt_nodes_irl +A collection of BT nodes for motion capabilities diff --git a/motion/include/ctrl_support/BTActionNode.hpp b/motion/include/ctrl_support/BTActionNode.hpp new file mode 100644 index 0000000..846c395 --- /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 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 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 // CTRL_SUPPORT__BTACTIONNODE_HPP_ diff --git a/motion/include/motion/base/Rotate.hpp b/motion/include/motion/base/Rotate.hpp new file mode 100644 index 0000000..4d307e9 --- /dev/null +++ b/motion/include/motion/base/Rotate.hpp @@ -0,0 +1,66 @@ +// 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__ROTATE_HPP_ +#define NAVIGATION__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 Rotate : public BT::ActionNodeBase +{ +public: + explicit Rotate(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 -1, it will spin forever + BT::InputPort("speed") + }); + } + +private: + std::shared_ptr node_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; + + double angle_, rotated_angle_, speed_; + + std::chrono::time_point last_time_; +}; + +} // namespace navigation + +#endif // NAVIGATION__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/NavigateThrough.hpp b/motion/include/motion/navigation/NavigateThrough.hpp new file mode 100644 index 0000000..eaa0423 --- /dev/null +++ b/motion/include/motion/navigation/NavigateThrough.hpp @@ -0,0 +1,79 @@ +// 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") + }); + } + +private: + std::vector generate_poses_to_goal( + const geometry_msgs::msg::PoseStamped & goal, + geometry_msgs::msg::TransformStamped map_to_goal, + int n_poses, const std::string & style); + + std::shared_ptr node_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_executor_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + double distance_tolerance_; + std::string tf_frame_, xml_path_; + geometry_msgs::msg::PoseStamped pose_; + +}; + +} // namespace navigation + +#endif // NAVIGATION__NAVIGATE_THROUGH_HPP_ diff --git a/motion/include/motion/navigation/NavigateTo.hpp b/motion/include/motion/navigation/NavigateTo.hpp new file mode 100644 index 0000000..f8681c1 --- /dev/null +++ b/motion/include/motion/navigation/NavigateTo.hpp @@ -0,0 +1,96 @@ +// 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 "navigation_system_interfaces/srv/set_truncate_distance.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("will_finish"), + BT::InputPort("is_truncated") + }); + } + +private: + std::shared_ptr node_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_executor_; + rclcpp::Client::SharedPtr + set_truncate_distance_client_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + double distance_tolerance_; + std::string tf_frame_, xml_path_; + geometry_msgs::msg::PoseStamped pose_; + + bool will_finish_{true}; + bool is_truncated_{false}; + + // std::shared_ptr> compute_action_client_; + // std::shared_ptr> follow_action_client_; + // rclcpp_action::ClientGoalHandle::WrappedResult path_result_; + // rclcpp_action::ClientGoalHandle::WrappedResult follow_result_; + // rclcpp_action::ClientGoalHandle::SharedPtr path_goal_handle_; + // rclcpp_action::ClientGoalHandle::SharedPtr follow_goal_handle_; + // bool path_result_available_, goal_send_ {false}; +}; + +} // 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/package.xml b/motion/package.xml new file mode 100644 index 0000000..6f17544 --- /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 + nav2_behavior_tree + nav2_costmap_2d + navigation_system_interfaces + slam_toolbox + + ament_lint_auto + ament_lint_common + + + + ament_cmake + + + + diff --git a/motion/src/motion/base/Rotate.cpp b/motion/src/motion/base/Rotate.cpp new file mode 100644 index 0000000..a156db9 --- /dev/null +++ b/motion/src/motion/base/Rotate.cpp @@ -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. + +#include "motion/base/Rotate.hpp" + +namespace base +{ + +Rotate::Rotate(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 Rotate::tick() +{ + RCLCPP_INFO(node_->get_logger(), "Rotate ticked"); + + getInput("angle", angle_); + getInput("speed", speed_); + + if (status() == BT::NodeStatus::IDLE) { + rotated_angle_ = 0; + last_time_ = std::chrono::high_resolution_clock::now(); + return BT::NodeStatus::RUNNING; + } + + geometry_msgs::msg::Twist cmd_vel; + cmd_vel.angular.z = speed_; + + if (angle_ < 0) { + vel_pub_->publish(cmd_vel); + return BT::NodeStatus::RUNNING; + } else if ((rotated_angle_ < angle_) && (angle_ >= 0)) { + 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; + last_time_ = curr_time; + return BT::NodeStatus::RUNNING; + } + + return BT::NodeStatus::FAILURE; +} + +void Rotate::halt() +{ + RCLCPP_INFO(node_->get_logger(), "Rotate halted"); +} + +} // namespace navigation + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Rotate"); +} diff --git a/motion/src/motion/head/Pan.cpp b/motion/src/motion/head/Pan.cpp new file mode 100644 index 0000000..c342b3c --- /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/NavigateThrough.cpp b/motion/src/motion/navigation/NavigateThrough.cpp new file mode 100644 index 0000000..9f62af7 --- /dev/null +++ b/motion/src/motion/navigation/NavigateThrough.cpp @@ -0,0 +1,147 @@ +// 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/NavigateThrough.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_) +{ + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); +} + +void +NavigateThrough::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "NavigateThrough ticked"); + geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::TransformStamped map_to_goal; + std::vector poses_to_goal; + int steps; + std::string trajectory_style; + getInput("tf_frame", tf_frame_); + getInput("n_poses", steps); + getInput("style", trajectory_style); + + 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.header.frame_id = "map"; + 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; + + poses_to_goal = generate_poses_to_goal(goal, map_to_goal, 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, + int n_poses, + const std::string & style) +{ + std::vector poses; + + 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 path + double fraction = static_cast(i) / (n_poses - 1); + + if (style == "straight") { + // Straight line between the initial and final poses + pose.pose.position.x = fraction * map_to_goal.transform.translation.x; + pose.pose.position.y = fraction * map_to_goal.transform.translation.y; + + } else if (style == "circular-l" || style == "circular-r") { + // Radius of the arc + double radius = std::sqrt( + map_to_goal.transform.translation.x * map_to_goal.transform.translation.x + + map_to_goal.transform.translation.y * map_to_goal.transform.translation.y) / 2.0; + double angle = fraction * M_PI; // Rotation of 180 degrees + + // Calculation of the center of the arc and application of the angle according to the direction + double sign = (style == "circular-l") ? 1.0 : -1.0; + pose.pose.position.x = radius * std::cos(angle) * sign; + pose.pose.position.y = radius * std::sin(angle); + } + + // Orientation remains stable + pose.pose.orientation = goal.pose.orientation; + + 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/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp new file mode 100644 index 0000000..8fedb2a --- /dev/null +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -0,0 +1,149 @@ +// 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/NavigateTo.hpp" + +namespace navigation +{ + +NavigateTo::NavigateTo( + const std::string & xml_tag_name, const std::string & action_name, + const BT::NodeConfiguration & conf) +: motion::BtActionNode< + nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode>( + xml_tag_name, action_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) +{ + // config().blackboard->get("node", node_); + + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + set_truncate_distance_client_ = + node_->create_client( + "navigation_system_node/set_truncate_distance", rmw_qos_profile_services_default, + callback_group_); +} + +void NavigateTo::on_tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "NavigateTo ticked"); + geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::TransformStamped map_to_goal; + + getInput("tf_frame", tf_frame_); + getInput("distance_tolerance", distance_tolerance_); + getInput("will_finish", will_finish_); + getInput("is_truncated", is_truncated_); + + 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.header.frame_id = "map"; + 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; + + if (!set_truncate_distance_client_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_WARN(node_->get_logger(), "Waiting for action server to be up..."); + setStatus(BT::NodeStatus::RUNNING); + } + + 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()); + + if (is_truncated_) { + xml_path_ = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance_); + + // auto request = std::make_shared(); + + // request->distance = distance_tolerance_; + // request->xml_content = nav_to_pose_truncated_xml; + // auto future_request = set_truncate_distance_client_->async_send_request(request).share(); + // if (rclcpp::spin_until_future_complete(node_, future_request) == + // rclcpp::FutureReturnCode::SUCCESS) + // { + // RCLCPP_INFO(node_->get_logger(), "Truncate distance setted"); + // auto result = *future_request.get(); + // if (!result.success) { + // RCLCPP_ERROR(node_->get_logger(), "Truncate distance FAILED calling service"); + // setStatus(BT::NodeStatus::FAILURE); + // } + // xml_path_ = result.xml_path; + // } else { + // RCLCPP_ERROR(node_->get_logger(), "Truncate distance FAILED"); + // setStatus(BT::NodeStatus::FAILURE); + // } + goal_.behavior_tree = xml_path_; + } + + goal_.pose = goal; +} + +BT::NodeStatus NavigateTo::on_success() +{ + RCLCPP_INFO(node_->get_logger(), "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(), "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 From 4dc79c1300de7c484ee75cdbcce6a168dcb30171 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 14 Nov 2024 15:36:59 +0100 Subject: [PATCH 04/60] minor --- motion/include/motion/base/Rotate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/include/motion/base/Rotate.hpp b/motion/include/motion/base/Rotate.hpp index 4d307e9..d2e41b4 100644 --- a/motion/include/motion/base/Rotate.hpp +++ b/motion/include/motion/base/Rotate.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Intelligent Robotics Lab - Gentlebots +// 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. From d2d24560c998e529ee87f71aa546f93e0b6edceb Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 18 Nov 2024 23:21:15 +0100 Subject: [PATCH 05/60] navigation nodes refined --- .../motion/navigation/NavigateThrough.hpp | 6 +- .../include/motion/navigation/NavigateTo.hpp | 16 +--- .../src/motion/navigation/NavigateThrough.cpp | 44 +++++++---- motion/src/motion/navigation/NavigateTo.cpp | 77 +++++++++---------- 4 files changed, 73 insertions(+), 70 deletions(-) diff --git a/motion/include/motion/navigation/NavigateThrough.hpp b/motion/include/motion/navigation/NavigateThrough.hpp index eaa0423..49f0de5 100644 --- a/motion/include/motion/navigation/NavigateThrough.hpp +++ b/motion/include/motion/navigation/NavigateThrough.hpp @@ -50,7 +50,9 @@ class NavigateThrough : public motion::BtActionNode< { BT::InputPort("tf_frame"), BT::InputPort("style"), // straight, circular-l, circular-r - BT::InputPort("n_poses") + BT::InputPort("n_poses"), + BT::InputPort("x"), + BT::InputPort("y"), }); } @@ -69,8 +71,6 @@ class NavigateThrough : public motion::BtActionNode< tf2_ros::TransformListener tf_listener_; double distance_tolerance_; - std::string tf_frame_, xml_path_; - geometry_msgs::msg::PoseStamped pose_; }; diff --git a/motion/include/motion/navigation/NavigateTo.hpp b/motion/include/motion/navigation/NavigateTo.hpp index f8681c1..7e21b5e 100644 --- a/motion/include/motion/navigation/NavigateTo.hpp +++ b/motion/include/motion/navigation/NavigateTo.hpp @@ -59,6 +59,8 @@ class NavigateTo : public motion::BtActionNode< { BT::InputPort("distance_tolerance"), BT::InputPort("tf_frame"), + BT::InputPort("x"), + BT::InputPort("y"), BT::InputPort("will_finish"), BT::InputPort("is_truncated") }); @@ -74,21 +76,9 @@ class NavigateTo : public motion::BtActionNode< tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; - - double distance_tolerance_; - std::string tf_frame_, xml_path_; - geometry_msgs::msg::PoseStamped pose_; - + bool will_finish_{true}; - bool is_truncated_{false}; - // std::shared_ptr> compute_action_client_; - // std::shared_ptr> follow_action_client_; - // rclcpp_action::ClientGoalHandle::WrappedResult path_result_; - // rclcpp_action::ClientGoalHandle::WrappedResult follow_result_; - // rclcpp_action::ClientGoalHandle::SharedPtr path_goal_handle_; - // rclcpp_action::ClientGoalHandle::SharedPtr follow_goal_handle_; - // bool path_result_available_, goal_send_ {false}; }; } // namespace navigation diff --git a/motion/src/motion/navigation/NavigateThrough.cpp b/motion/src/motion/navigation/NavigateThrough.cpp index 9f62af7..2038932 100644 --- a/motion/src/motion/navigation/NavigateThrough.cpp +++ b/motion/src/motion/navigation/NavigateThrough.cpp @@ -39,26 +39,42 @@ NavigateThrough::on_tick() geometry_msgs::msg::TransformStamped map_to_goal; std::vector poses_to_goal; int steps; - std::string trajectory_style; - getInput("tf_frame", tf_frame_); + std::string tf_frame, xml_path, trajectory_style; + + getInput("tf_frame", tf_frame); getInput("n_poses", steps); getInput("style", trajectory_style); - 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); + + 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", goal.pose.position.x); + getInput("y", goal.pose.position.y); + + RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %f, y: %f", goal.pose.position.x, goal.pose.position.y); + goal.pose.orientation.x = 0.0; + goal.pose.orientation.y = 0.0; + goal.pose.orientation.z = 0.0; + goal.pose.orientation.w = 1.0; } goal.header.frame_id = "map"; - 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; poses_to_goal = generate_poses_to_goal(goal, map_to_goal, steps, trajectory_style); diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index 8fedb2a..af4a5f9 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -26,7 +26,6 @@ NavigateTo::NavigateTo( tf_buffer_(), tf_listener_(tf_buffer_) { - // config().blackboard->get("node", node_); callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); @@ -42,27 +41,43 @@ void NavigateTo::on_tick() RCLCPP_DEBUG(node_->get_logger(), "NavigateTo ticked"); geometry_msgs::msg::PoseStamped goal; geometry_msgs::msg::TransformStamped map_to_goal; + bool is_truncated; + std::string tf_frame, xml_path; - getInput("tf_frame", tf_frame_); - getInput("distance_tolerance", distance_tolerance_); + getInput("tf_frame", tf_frame); getInput("will_finish", will_finish_); - getInput("is_truncated", is_truncated_); - - 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); + getInput("is_truncated", is_truncated); + + 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_WARN( + 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 { // No TF, use coordinates + + getInput("x", goal.pose.position.x); + getInput("y", goal.pose.position.y); + RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %f, y: %f", 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"; - 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; if (!set_truncate_distance_client_->wait_for_service(std::chrono::seconds(1))) { RCLCPP_WARN(node_->get_logger(), "Waiting for action server to be up..."); @@ -74,29 +89,11 @@ void NavigateTo::on_tick() 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()); - if (is_truncated_) { - xml_path_ = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance_); - - // auto request = std::make_shared(); - - // request->distance = distance_tolerance_; - // request->xml_content = nav_to_pose_truncated_xml; - // auto future_request = set_truncate_distance_client_->async_send_request(request).share(); - // if (rclcpp::spin_until_future_complete(node_, future_request) == - // rclcpp::FutureReturnCode::SUCCESS) - // { - // RCLCPP_INFO(node_->get_logger(), "Truncate distance setted"); - // auto result = *future_request.get(); - // if (!result.success) { - // RCLCPP_ERROR(node_->get_logger(), "Truncate distance FAILED calling service"); - // setStatus(BT::NodeStatus::FAILURE); - // } - // xml_path_ = result.xml_path; - // } else { - // RCLCPP_ERROR(node_->get_logger(), "Truncate distance FAILED"); - // setStatus(BT::NodeStatus::FAILURE); - // } - goal_.behavior_tree = xml_path_; + if (is_truncated) { + double distance_tolerance; + getInput("distance_tolerance", distance_tolerance); + xml_path = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance); + goal_.behavior_tree = xml_path; } goal_.pose = goal; From 0cc7f15b1121c91b2b5eec0bee3903f9db4d1b3b Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 19 Nov 2024 17:07:40 +0100 Subject: [PATCH 06/60] refined --- hri/launch/hri.launch.py | 4 +- hri/src/hri/main_hri.cpp | 4 +- motion/CMakeLists.txt | 10 ++- motion/bt_xml/moveto.xml | 19 +++++ motion/bt_xml/simple_nav.xml | 20 +++++ motion/config/nav.yaml | 8 ++ motion/include/ctrl_support/BTActionNode.hpp | 6 +- .../include/motion/navigation/NavigateTo.hpp | 5 +- motion/launch/nav.launch.py | 50 +++++++++++++ motion/package.xml | 1 - motion/src/motion/main_nav.cpp | 75 +++++++++++++++++++ motion/src/motion/navigation/NavigateTo.cpp | 26 ++----- 12 files changed, 196 insertions(+), 32 deletions(-) create mode 100644 motion/bt_xml/moveto.xml create mode 100644 motion/bt_xml/simple_nav.xml create mode 100644 motion/config/nav.yaml create mode 100644 motion/launch/nav.launch.py create mode 100644 motion/src/motion/main_nav.cpp diff --git a/hri/launch/hri.launch.py b/hri/launch/hri.launch.py index e6ed9d0..330f661 100644 --- a/hri/launch/hri.launch.py +++ b/hri/launch/hri.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): # Get the launch directory - pkg_dir = get_package_share_directory('hri') + pkg_dir = get_package_share_directory('hri_bt_nodes') params_file = os.path.join(pkg_dir, 'config', 'hri.yaml') # print('params_file: ', params_file) @@ -34,7 +34,7 @@ def generate_launch_description(): ld = LaunchDescription() hri_cmd = Node( - package='hri', + package='hri_bt_nodes', executable='hri_test', output='screen', remappings=[ diff --git a/hri/src/hri/main_hri.cpp b/hri/src/hri/main_hri.cpp index 8885cb1..6651db2 100644 --- a/hri/src/hri/main_hri.cpp +++ b/hri/src/hri/main_hri.cpp @@ -46,7 +46,7 @@ int main(int argc, char * argv[]) factory.registerFromPlugin(loader.getOSName(plugin)); } - std::string pkgpath = ament_index_cpp::get_package_share_directory("hri"); + 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()); @@ -68,6 +68,8 @@ int main(int argc, char * argv[]) rate.sleep(); } + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + rclcpp::shutdown(); return 0; } diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt index 87656ee..d051e2b 100644 --- a/motion/CMakeLists.txt +++ b/motion/CMakeLists.txt @@ -67,7 +67,12 @@ foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() +add_executable(nav_test src/motion/main_nav.cpp) +ament_target_dependencies(nav_test ${dependencies}) +target_link_libraries(nav_test ${ZMQ_LIBRARIES}) + install(TARGETS + nav_test ${plugin_libs} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib @@ -75,9 +80,10 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ - DESTINATION include/ +install(DIRECTORY include launch config bt_xml + DESTINATION share/${PROJECT_NAME} ) + # install(FILES clear_people_layer.xml # DESTINATION share/${PROJECT_NAME} # ) diff --git a/motion/bt_xml/moveto.xml b/motion/bt_xml/moveto.xml new file mode 100644 index 0000000..b388aef --- /dev/null +++ b/motion/bt_xml/moveto.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/simple_nav.xml b/motion/bt_xml/simple_nav.xml new file mode 100644 index 0000000..10557df --- /dev/null +++ b/motion/bt_xml/simple_nav.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/motion/config/nav.yaml b/motion/config/nav.yaml new file mode 100644 index 0000000..c4dfffc --- /dev/null +++ b/motion/config/nav.yaml @@ -0,0 +1,8 @@ +nav_node: + ros__parameters: + use_sim_time: True + bt_xml_file: simple_nav.xml + plugins: + - navigate_to_bt_node + - move_to_bt_node + diff --git a/motion/include/ctrl_support/BTActionNode.hpp b/motion/include/ctrl_support/BTActionNode.hpp index 846c395..98c3056 100644 --- a/motion/include/ctrl_support/BTActionNode.hpp +++ b/motion/include/ctrl_support/BTActionNode.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CTRL_SUPPORT__BTACTIONNODE_HPP_ -#define CTRL_SUPPORT__BTACTIONNODE_HPP_ +#ifndef NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ +#define NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ #include #include @@ -272,4 +272,4 @@ class BtActionNode : public BT::ActionNodeBase } // namespace motion -#endif // CTRL_SUPPORT__BTACTIONNODE_HPP_ +#endif // NAVIGATION__CTRL_SUPPORT__BTACTIONNODE_HPP_ \ No newline at end of file diff --git a/motion/include/motion/navigation/NavigateTo.hpp b/motion/include/motion/navigation/NavigateTo.hpp index 7e21b5e..5b02031 100644 --- a/motion/include/motion/navigation/NavigateTo.hpp +++ b/motion/include/motion/navigation/NavigateTo.hpp @@ -61,8 +61,7 @@ class NavigateTo : public motion::BtActionNode< BT::InputPort("tf_frame"), BT::InputPort("x"), BT::InputPort("y"), - BT::InputPort("will_finish"), - BT::InputPort("is_truncated") + BT::InputPort("will_finish") }); } @@ -71,8 +70,6 @@ class NavigateTo : public motion::BtActionNode< rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_executor_; - rclcpp::Client::SharedPtr - set_truncate_distance_client_; tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/motion/launch/nav.launch.py b/motion/launch/nav.launch.py new file mode 100644 index 0000000..0c7707b --- /dev/null +++ b/motion/launch/nav.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', 'nav.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='nav_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 index 6f17544..e3c7712 100644 --- a/motion/package.xml +++ b/motion/package.xml @@ -16,7 +16,6 @@ behaviortree_cpp_v3 geometry_msgs nav2_msgs - nav2_behavior_tree nav2_costmap_2d navigation_system_interfaces slam_toolbox diff --git a/motion/src/motion/main_nav.cpp b/motion/src/motion/main_nav.cpp new file mode 100644 index 0000000..8eeec64 --- /dev/null +++ b/motion/src/motion/main_nav.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("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(); + } + + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); + + rclcpp::shutdown(); + return 0; +} diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index af4a5f9..65344df 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -20,20 +20,12 @@ namespace navigation NavigateTo::NavigateTo( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: motion::BtActionNode< - nav2_msgs::action::NavigateToPose, rclcpp_cascade_lifecycle::CascadeLifecycleNode>( +: motion::BtActionNode( xml_tag_name, action_name, conf), tf_buffer_(), tf_listener_(tf_buffer_) { - - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - - set_truncate_distance_client_ = - node_->create_client( - "navigation_system_node/set_truncate_distance", rmw_qos_profile_services_default, - callback_group_); + config().blackboard->get("node", node_); } void NavigateTo::on_tick() @@ -79,19 +71,15 @@ void NavigateTo::on_tick() goal.header.frame_id = "map"; - if (!set_truncate_distance_client_->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_WARN(node_->get_logger(), "Waiting for action server to be up..."); - setStatus(BT::NodeStatus::RUNNING); - } - 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()); - if (is_truncated) { - double distance_tolerance; - getInput("distance_tolerance", distance_tolerance); + double distance_tolerance = 0; + getInput("distance_tolerance", distance_tolerance); + + if (distance_tolerance != 0) { xml_path = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance); goal_.behavior_tree = xml_path; } @@ -139,7 +127,7 @@ BT::NodeStatus NavigateTo::on_cancelled() BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { - return std::make_unique(name, "/navigate_to_pose", config); + return std::make_unique(name, "navigate_to_pose", config); }; factory.registerBuilder("NavigateTo", builder); From 3bfb8e388d2d747787fa8685643f6dde8398577a Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 19 Nov 2024 17:08:18 +0100 Subject: [PATCH 07/60] perception --- .gitignore | 3 + perception/CMakeLists.txt | 97 ++++++++++ perception/README.md | 2 + .../include/perception/FilterEntity.hpp | 81 +++++++++ .../include/perception/IdentifyPerson.hpp | 71 ++++++++ perception/include/perception/IsDetected.hpp | 77 ++++++++ perception/include/perception/IsPointing.hpp | 83 +++++++++ perception/package.xml | 38 ++++ perception/src/perception/FilterEntity.cpp | 106 +++++++++++ perception/src/perception/IdentifyPerson.cpp | 78 ++++++++ perception/src/perception/IsDetected.cpp | 128 +++++++++++++ perception/src/perception/IsPointing.cpp | 170 ++++++++++++++++++ perception/thirdparty.repos | 9 + 13 files changed, 943 insertions(+) create mode 100644 .gitignore create mode 100644 perception/CMakeLists.txt create mode 100644 perception/README.md create mode 100644 perception/include/perception/FilterEntity.hpp create mode 100644 perception/include/perception/IdentifyPerson.hpp create mode 100644 perception/include/perception/IsDetected.hpp create mode 100644 perception/include/perception/IsPointing.hpp create mode 100644 perception/package.xml create mode 100644 perception/src/perception/FilterEntity.cpp create mode 100644 perception/src/perception/IdentifyPerson.cpp create mode 100644 perception/src/perception/IsDetected.cpp create mode 100644 perception/src/perception/IsPointing.cpp create mode 100644 perception/thirdparty.repos diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4ef01bc --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.gitingore +_*.hpp +_*.cpp \ No newline at end of file diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt new file mode 100644 index 0000000..9b20ec6 --- /dev/null +++ b/perception/CMakeLists.txt @@ -0,0 +1,97 @@ +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(rclcpp REQUIRED) +find_package(rclcpp_cascade_lifecycle REQUIRED) +find_package(behaviortree_cpp_v3 REQUIRED) +find_package(yolov8_msgs 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) +# find_package(backward_ros REQUIRED) + + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + rclcpp + rclcpp_cascade_lifecycle + rclpy + behaviortree_cpp_v3 + yolov8_msgs + tf2_ros + geometry_msgs + tf2_geometry_msgs + tf2 + rclcpp_action + behaviortree_cpp_v3 + perception_system + OpenCV + perception_system_interfaces + sensor_msgs + std_srvs + # backward_ros +) + +include_directories(include) + + +add_library(is_detected_bt_node SHARED src/perception/IsDetected.cpp) +add_library(filter_entity_bt_node SHARED src/perception/FilterEntity.cpp) +add_library(is_pointing_bt_node SHARED src/perception/IsPointing.cpp) +add_library(identify_person_bt_node SHARED src/perception/IdentifyPerson.cpp) + +list(APPEND plugin_libs + is_detected_bt_node + filter_entity_bt_node + is_pointing_bt_node + identify_person_bt_node +) + +foreach(bt_plugin ${plugin_libs}) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +endforeach() + +install(TARGETS + ${plugin_libs} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(PROGRAMS + DESTINATION lib/${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_dependencies(${dependencies}) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_package() diff --git a/perception/README.md b/perception/README.md new file mode 100644 index 0000000..cff43c2 --- /dev/null +++ b/perception/README.md @@ -0,0 +1,2 @@ +# perception_bt_nodes_irl +A collection of BT nodes for perception capabilities diff --git a/perception/include/perception/FilterEntity.hpp b/perception/include/perception/FilterEntity.hpp new file mode 100644 index 0000000..ab002a2 --- /dev/null +++ b/perception/include/perception/FilterEntity.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"), + BT::OutputPort("filtered_frame"), + }); + } + +private: + 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_; + + geometry_msgs::msg::TransformStamped initialize_state_observer( + const geometry_msgs::msg::TransformStamped & entity); + geometry_msgs::msg::TransformStamped update_state_observer( + const geometry_msgs::msg::TransformStamped & entity); +}; + +} // namespace perception + +#endif // PERCEPTION__FILTER_ENTITY_HPP_ diff --git a/perception/include/perception/IdentifyPerson.hpp b/perception/include/perception/IdentifyPerson.hpp new file mode 100644 index 0000000..ee3100c --- /dev/null +++ b/perception/include/perception/IdentifyPerson.hpp @@ -0,0 +1,71 @@ +// 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_PERSON_HPP_ +#define IDENTIFY_PERSON_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 IdentifyPerson : public BT::ActionNodeBase +{ +public: + IdentifyPerson(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("person_to_identify"), + BT::InputPort("get_features") // If true, features are saved. If false, features were already saved + }); + } + +private: + std::shared_ptr node_; + std::shared_ptr static_broadcaster_; + + std::string person_; + bool get_features_; + + // tf2::BufferCore tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + +}; + +} // namespace perception + +#endif // IDENTIFY_PERSON_HPP_ diff --git a/perception/include/perception/IsDetected.hpp b/perception/include/perception/IsDetected.hpp new file mode 100644 index 0000000..8814a58 --- /dev/null +++ b/perception/include/perception/IsDetected.hpp @@ -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. + +#ifndef PERCEPTION__ISDETECTED_HPP_ +#define PERCEPTION__ISDETECTED_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/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "perception_system/PerceptionListener.hpp" +#include "perception_system_interfaces/msg/detection_array.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"), + BT::InputPort("person_id"), + BT::InputPort("cam_frame"), + BT::InputPort("interest"), + BT::InputPort("confidence"), + BT::InputPort("order"), + BT::InputPort("max_depth"), + BT::OutputPort>("frames"), // TF frames of the detected entities + BT::OutputPort("best_detection") + }); + } + +private: + std::shared_ptr node_; + // std::shared_ptr tf_buffer_; + + std::string interest_, order_, cam_frame_; + 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/IsPointing.hpp b/perception/include/perception/IsPointing.hpp new file mode 100644 index 0000000..f50dd83 --- /dev/null +++ b/perception/include/perception/IsPointing.hpp @@ -0,0 +1,83 @@ +// 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 + +#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 "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("person_id"), + BT::InputPort("camera_frame"), + BT::InputPort("low_pointing_limit"), + BT::InputPort("high_pointing_limit"), + BT::OutputPort("output_frame"), + BT::OutputPort("pointing_direction"), + BT::OutputPort("person_id") + }); + } + +private: + int publish_detection_tf(const perception_system_interfaces::msg::Detection & detection); + + std::shared_ptr node_; + + std::string camera_frame_, output_frame_; + int low_pointing_limit_, high_pointing_limit_; + int pointing_direction_; + + // std::int64_t person_id_; + + geometry_msgs::msg::TransformStamped person_pose_; + rclcpp::Time last_pose_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_static_broadcaster_; +}; + +} // namespace perception + +#endif // PERCEPTION__IS_POINTING_HPP_ diff --git a/perception/package.xml b/perception/package.xml new file mode 100644 index 0000000..18ac62f --- /dev/null +++ b/perception/package.xml @@ -0,0 +1,38 @@ + + + + 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 + yolov8_msgs + moveit_msgs + shape_msgs + rclpy + tf2_ros + geometry_msgs + tf2_geometry_msgs + tf2 + rclcpp_action + perception_system + perception_system_interfaces + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/perception/src/perception/FilterEntity.cpp b/perception/src/perception/FilterEntity.cpp new file mode 100644 index 0000000..dc33bfb --- /dev/null +++ b/perception/src/perception/FilterEntity.cpp @@ -0,0 +1,106 @@ +// 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/FilterEntity.hpp" + +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" + +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_); + setOutput("filtered_frame", frame_ + "_filtered"); + 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(), "IsMoving filtering frame %s", frame_.c_str()); + + geometry_msgs::msg::TransformStamped entity_transform_now_msg; + + try { + entity_transform_now_msg = tf_buffer_->lookupTransform("odom", frame_, tf2::TimePointZero); + RCLCPP_INFO( + node_->get_logger(), "Position %s to %s: %f %f %f", frame_.c_str(), "odom", + entity_transform_now_msg.transform.translation.x, + entity_transform_now_msg.transform.translation.y, + entity_transform_now_msg.transform.translation.z); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + node_->get_logger(), "Could not transform %s to %s: %s", frame_.c_str(), "odom", ex.what()); + RCLCPP_INFO(node_->get_logger(), "Cannot transform"); + + return BT::NodeStatus::SUCCESS; + } + geometry_msgs::msg::TransformStamped filtered_entity; + if (state_obs_initialized_) { + filtered_entity = update_state_observer(entity_transform_now_msg); + } else { + filtered_entity = initialize_state_observer(entity_transform_now_msg); + state_obs_initialized_ = true; + } + filtered_entity.child_frame_id = frame_ + "_filtered"; + tf_broadcaster_->sendTransform(filtered_entity); + setOutput("filtered_frame", filtered_entity.child_frame_id); + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("FilterEntity"); +} diff --git a/perception/src/perception/IdentifyPerson.cpp b/perception/src/perception/IdentifyPerson.cpp new file mode 100644 index 0000000..78901dd --- /dev/null +++ b/perception/src/perception/IdentifyPerson.cpp @@ -0,0 +1,78 @@ +// 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/IdentifyPerson.hpp" + +namespace perception +{ + +using pl = perception_system::PerceptionListener; + +IdentifyPerson::IdentifyPerson( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf), + person_("") + // get_features_(false), + // tf_buffer_(), + // tf_listener_(tf_buffer_) +{ + config().blackboard->get("node", node_); + getInput("person_to_identify", person_); + getInput("get_features", get_features_); + static_broadcaster_ = std::make_shared(node_); +} + +BT::NodeStatus +IdentifyPerson::tick() +{ + try + { + // // Get the transform from map to the person + // auto map2person_msg = tf_buffer_.lookupTransform("map", person_, tf2::TimePointZero); + // tf2::Stamped map2person; + // tf2::fromMsg(map2person_msg, map2person); + + // // Publish transform + // static_broadcaster_->sendTransform(tf2::toMsg(map2person)); + + if (get_features_) // Configure perception system to track the specified person + { + // pl::getInstance(node_)->set_features_of_interest(person_); + } + // Get the detection of the person with the previously configured features + perception_system_interfaces::msg::Detection detection; + // detection = pl::getInstance(node_)->get_by_features(person_); + // Publish the detection + pl::getInstance(node_)->publicTF(detection, person_); + + return BT::NodeStatus::SUCCESS; + } + catch(const std::exception& ex) + { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + return BT::NodeStatus::FAILURE; + } + + return BT::NodeStatus::RUNNING; +} + + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IdentifyPerson"); +} + diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/IsDetected.cpp new file mode 100644 index 0000000..ff256f1 --- /dev/null +++ b/perception/src/perception/IsDetected.cpp @@ -0,0 +1,128 @@ +// 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/IsDetected.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_); + + // node_->add_activation("perception_system/perception_people_detection") + + getInput("interest", interest_); + getInput("cam_frame", cam_frame_); + getInput("confidence", threshold_); + getInput("max_entities", max_entities_); + getInput("order", order_); + getInput("max_depth", max_depth_); + getInput("person_id", person_id_); +} + +BT::NodeStatus IsDetected::tick() +{ + rclcpp::spin_some(node_->get_node_base_interface()); + getInput("person_id", person_id_); + + if (status() == BT::NodeStatus::IDLE) { + RCLCPP_INFO(node_->get_logger(), "IsDetected ticked while IDLE"); + } + + RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked"); + 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_WARN(node_->get_logger(), "[IsDetected] No detections"); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] 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(), "[IsDetected] 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(), "[IsDetected] 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); + + setOutput("best_detection", detections[0].class_name); + RCLCPP_INFO(node_->get_logger(), "[IsDetected] Detections sorted"); + + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Max Depth: %f", max_depth_); + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] 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(), "[IsDetected] Removing detection %s", detection.class_name.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Score: %f", detection.score); + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Depth: %f", detection.center3d.position.z); + it = detections.erase(it); + + } else { + frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter)); + if (pl::getInstance(node_)->publicTF(detection, std::to_string(entity_counter)) == -1) { + return BT::NodeStatus::FAILURE; + } + ++it; + ++entity_counter; + } + } + + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections sorted and filtered"); + if (frames_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "[IsDetected] No detections after filter"); + return BT::NodeStatus::FAILURE; + } + + setOutput("frames", frames_); + frames_.clear(); + + RCLCPP_DEBUG(node_->get_logger(), "Pointing direction: %d", detections[0].pointing_direction); + + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections published"); + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsDetected"); +} diff --git a/perception/src/perception/IsPointing.cpp b/perception/src/perception/IsPointing.cpp new file mode 100644 index 0000000..fb1271d --- /dev/null +++ b/perception/src/perception/IsPointing.cpp @@ -0,0 +1,170 @@ +// 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/IsPointing.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), + tf_buffer_() +{ + config().blackboard->get("node", node_); + + getInput("camera_frame", camera_frame_); + getInput("low_pointing_limit", low_pointing_limit_); + getInput("high_pointing_limit", high_pointing_limit_); + getInput("output_frame", output_frame_); + + tf_static_broadcaster_ = std::make_shared(node_); +} + +// // Distance between two transformations +// double tfs_distance( +// const geometry_msgs::msg::TransformStamped & tf1, +// const geometry_msgs::msg::TransformStamped & tf2) +// { +// return sqrt( +// pow(tf1.transform.translation.x - tf2.transform.translation.x, 2) + +// pow(tf1.transform.translation.y - tf2.transform.translation.y, 2) + +// pow(tf1.transform.translation.z - tf2.transform.translation.z, 2)); +// } + +// // Mean of two transformations +// geometry_msgs::msg::TransformStamped tfs_mean( +// const geometry_msgs::msg::TransformStamped & tf1, +// const geometry_msgs::msg::TransformStamped & tf2) +// { +// geometry_msgs::msg::TransformStamped mean = tf1; + +// mean.transform.translation.x = (tf1.transform.translation.x + tf2.transform.translation.x) / 2; +// mean.transform.translation.y = (tf1.transform.translation.y + tf2.transform.translation.y) / 2; +// mean.transform.translation.z = (tf1.transform.translation.z + tf2.transform.translation.z) / 2; + +// return mean; +// } + +int IsPointing::publish_detection_tf( + const perception_system_interfaces::msg::Detection & detection) +{ + int pointing_direction = -1; + geometry_msgs::msg::TransformStamped map2camera_msg; + + RCLCPP_INFO( + node_->get_logger(), "[IsPointing] Detectection in frame_id %s", + detection.header.frame_id.c_str()); + + try { + map2camera_msg = tf_buffer_->lookupTransform("map", camera_frame_, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + node_->get_logger(), "[IsPointing] Could not transform %s to %s: %s", "map", + camera_frame_.c_str(), ex.what()); + return -1; + } + + // 0 is right, 1 is down-right, 2 is down, 3 is down-left, 4 is left, 5 is up-left, 6 is up, 7 is up-right + RCLCPP_INFO( + node_->get_logger(), "[IsPointing] Low pointing limit %d. High point limit %d", low_pointing_limit_, + high_pointing_limit_); + for (int i = low_pointing_limit_; i <= high_pointing_limit_; i++) { + RCLCPP_INFO( + node_->get_logger(), "[IsPointing] Pointing direction %d", + detection.pointing_direction); + if (detection.pointing_direction == i) { + pointing_direction = i; + + tf2::Transform camera2detection; + camera2detection.setOrigin( + tf2::Vector3( + detection.center3d.position.x, detection.center3d.position.y, + detection.center3d.position.z)); + camera2detection.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + + tf2::Transform map2camera; + tf2::fromMsg(map2camera_msg.transform, map2camera); + + tf2::Transform map2detection = map2camera * camera2detection; + + geometry_msgs::msg::TransformStamped map2detection_msg; + map2detection_msg.header.frame_id = "map"; + map2detection_msg.child_frame_id = output_frame_; + + map2detection_msg.transform = tf2::toMsg(map2detection); + map2detection_msg.transform.translation.z = 0.0; + + tf_static_broadcaster_->sendTransform(map2detection_msg); + break; + } + } + return pointing_direction; +} + +BT::NodeStatus IsPointing::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + RCLCPP_INFO(node_->get_logger(), "IsPointing ticked while IDLE"); + } + + pl::getInstance(node_)->set_interest("person", true); + pl::getInstance(node_)->update(true); + rclcpp::spin_some(node_->get_node_base_interface()); + + std::vector detections; + detections = pl::getInstance(node_)->get_by_type("person"); + + if (detections.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections"); + return BT::NodeStatus::FAILURE; + } + + perception_system_interfaces::msg::Detection best_detection; + + // 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); + // }); + + best_detection = detections[0]; + + RCLCPP_INFO( + node_->get_logger(), "[IsPointing] Best detection: %s, color: %ld, pointing: %d", + best_detection.unique_id.c_str(), best_detection.color_person, + best_detection.pointing_direction); + + int direction = publish_detection_tf(best_detection); + if (direction == -1) { + RCLCPP_ERROR(node_->get_logger(), "[IsPointing] Error, invalid pointing direction"); + return BT::NodeStatus::FAILURE; + } + setOutput("output_frame", "someone_pointing"); + setOutput("pointing_direction", direction); + setOutput("person_id", best_detection.unique_id); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsPointing"); +} 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 From 16f6ac51189317d92ae5b5dffede3d1df8a4af98 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 19 Nov 2024 20:43:20 +0100 Subject: [PATCH 08/60] motion nodes refined --- motion/bt_xml/{simple_nav.xml => nav.xml} | 1 + motion/bt_xml/nav_through.xml | 30 +++++++ .../bt_xml/{moveto.xml => nav_truncate.xml} | 8 +- motion/config/nav.yaml | 5 +- .../motion/navigation/NavigateThrough.hpp | 5 +- .../src/motion/navigation/NavigateThrough.cpp | 82 +++++++++++++------ motion/src/motion/navigation/NavigateTo.cpp | 13 ++- 7 files changed, 101 insertions(+), 43 deletions(-) rename motion/bt_xml/{simple_nav.xml => nav.xml} (85%) create mode 100644 motion/bt_xml/nav_through.xml rename motion/bt_xml/{moveto.xml => nav_truncate.xml} (57%) diff --git a/motion/bt_xml/simple_nav.xml b/motion/bt_xml/nav.xml similarity index 85% rename from motion/bt_xml/simple_nav.xml rename to motion/bt_xml/nav.xml index 10557df..c08a2fc 100644 --- a/motion/bt_xml/simple_nav.xml +++ b/motion/bt_xml/nav.xml @@ -4,6 +4,7 @@ + diff --git a/motion/bt_xml/nav_through.xml b/motion/bt_xml/nav_through.xml new file mode 100644 index 0000000..0f3401a --- /dev/null +++ b/motion/bt_xml/nav_through.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion/bt_xml/moveto.xml b/motion/bt_xml/nav_truncate.xml similarity index 57% rename from motion/bt_xml/moveto.xml rename to motion/bt_xml/nav_truncate.xml index b388aef..5359d68 100644 --- a/motion/bt_xml/moveto.xml +++ b/motion/bt_xml/nav_truncate.xml @@ -3,16 +3,18 @@ - + + - + - + + diff --git a/motion/config/nav.yaml b/motion/config/nav.yaml index c4dfffc..f8cb26f 100644 --- a/motion/config/nav.yaml +++ b/motion/config/nav.yaml @@ -1,8 +1,7 @@ nav_node: ros__parameters: use_sim_time: True - bt_xml_file: simple_nav.xml + bt_xml_file: nav_through.xml plugins: - navigate_to_bt_node - - move_to_bt_node - + - navigate_through_bt_node diff --git a/motion/include/motion/navigation/NavigateThrough.hpp b/motion/include/motion/navigation/NavigateThrough.hpp index 49f0de5..fec8ce6 100644 --- a/motion/include/motion/navigation/NavigateThrough.hpp +++ b/motion/include/motion/navigation/NavigateThrough.hpp @@ -51,6 +51,7 @@ class NavigateThrough : public motion::BtActionNode< BT::InputPort("tf_frame"), BT::InputPort("style"), // straight, circular-l, circular-r BT::InputPort("n_poses"), + // BT::InputPort("radius"), BT::InputPort("x"), BT::InputPort("y"), }); @@ -60,13 +61,11 @@ class NavigateThrough : public motion::BtActionNode< 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_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_executor_; - tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/motion/src/motion/navigation/NavigateThrough.cpp b/motion/src/motion/navigation/NavigateThrough.cpp index 2038932..db4da8d 100644 --- a/motion/src/motion/navigation/NavigateThrough.cpp +++ b/motion/src/motion/navigation/NavigateThrough.cpp @@ -27,8 +27,7 @@ NavigateThrough::NavigateThrough( tf_buffer_(), tf_listener_(tf_buffer_) { - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + config().blackboard->get("node", node_); } void @@ -36,7 +35,7 @@ NavigateThrough::on_tick() { RCLCPP_DEBUG(node_->get_logger(), "NavigateThrough ticked"); geometry_msgs::msg::PoseStamped goal; - geometry_msgs::msg::TransformStamped map_to_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; @@ -45,7 +44,7 @@ NavigateThrough::on_tick() 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 { @@ -55,7 +54,7 @@ NavigateThrough::on_tick() 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; @@ -64,19 +63,34 @@ NavigateThrough::on_tick() goal.pose.orientation.w = map_to_goal.transform.rotation.w; } else { - getInput("x", goal.pose.position.x); - getInput("y", goal.pose.position.y); + 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"); - RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %f, y: %f", goal.pose.position.x, goal.pose.position.y); 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"; - poses_to_goal = generate_poses_to_goal(goal, map_to_goal, steps, trajectory_style); + 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", @@ -105,10 +119,12 @@ 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) { @@ -117,34 +133,48 @@ NavigateThrough::generate_poses_to_goal( 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(); + pose.header.stamp = node_->now(); - // Fraction of the path + // Fracción del camino (0.0 a 1.0) double fraction = static_cast(i) / (n_poses - 1); if (style == "straight") { - // Straight line between the initial and final poses - pose.pose.position.x = fraction * map_to_goal.transform.translation.x; - pose.pose.position.y = fraction * map_to_goal.transform.translation.y; + // Interpolación lineal directa entre robot y goal + pose.pose.position.x = map_to_robot.transform.translation.x + + fraction * (map_to_goal.transform.translation.x - map_to_robot.transform.translation.x); + pose.pose.position.y = map_to_robot.transform.translation.y + + fraction * (map_to_goal.transform.translation.y - map_to_robot.transform.translation.y); + 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") { - // Radius of the arc - double radius = std::sqrt( - map_to_goal.transform.translation.x * map_to_goal.transform.translation.x + - map_to_goal.transform.translation.y * map_to_goal.transform.translation.y) / 2.0; - double angle = fraction * M_PI; // Rotation of 180 degrees - - // Calculation of the center of the arc and application of the angle according to the direction - double sign = (style == "circular-l") ? 1.0 : -1.0; - pose.pose.position.x = radius * std::cos(angle) * sign; - pose.pose.position.y = radius * std::sin(angle); + double radius = 1.0; + // getInput("radius", radius); + double angle = fraction * M_PI; // Desde 0 hasta 180 grados + 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; // Mantén z constante } - // Orientation remains stable + // Mantén la orientación del objetivo 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; } diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index 65344df..323877e 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -33,12 +33,13 @@ void NavigateTo::on_tick() RCLCPP_DEBUG(node_->get_logger(), "NavigateTo ticked"); geometry_msgs::msg::PoseStamped goal; geometry_msgs::msg::TransformStamped map_to_goal; - bool is_truncated; std::string tf_frame, xml_path; getInput("tf_frame", tf_frame); getInput("will_finish", will_finish_); - getInput("is_truncated", is_truncated); + + 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(), "Transforming %s to %s", "map", tf_frame.c_str()); @@ -72,14 +73,10 @@ void NavigateTo::on_tick() goal.header.frame_id = "map"; 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()); - - double distance_tolerance = 0; - getInput("distance_tolerance", distance_tolerance); + node_->get_logger(), "Sending coordinates. Frame: %s", goal.header.frame_id.c_str()); if (distance_tolerance != 0) { + RCLCPP_INFO(node_->get_logger(), "Setting distance tolerance to %f", distance_tolerance); xml_path = generate_xml_file(nav_to_pose_truncated_xml, distance_tolerance); goal_.behavior_tree = xml_path; } From 41b0d7c8c92bb62b85fa89f9271908e5591e3b33 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 19 Nov 2024 22:34:30 +0100 Subject: [PATCH 09/60] readme --- hri/README.md | 2 ++ motion/README.md | 19 ++++++++++- motion/bt_xml/nav_through.xml | 4 +-- .../motion/navigation/NavigateThrough.hpp | 2 +- .../src/motion/navigation/NavigateThrough.cpp | 33 ++++++++++++------- perception/README.md | 2 +- 6 files changed, 46 insertions(+), 16 deletions(-) create mode 100644 hri/README.md 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/motion/README.md b/motion/README.md index 5952267..92335a3 100644 --- a/motion/README.md +++ b/motion/README.md @@ -1,2 +1,19 @@ -# motion_bt_nodes_irl +# 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/nav_through.xml b/motion/bt_xml/nav_through.xml index 0f3401a..96b5d53 100644 --- a/motion/bt_xml/nav_through.xml +++ b/motion/bt_xml/nav_through.xml @@ -4,7 +4,7 @@ - + @@ -13,7 +13,7 @@ - + diff --git a/motion/include/motion/navigation/NavigateThrough.hpp b/motion/include/motion/navigation/NavigateThrough.hpp index fec8ce6..389c930 100644 --- a/motion/include/motion/navigation/NavigateThrough.hpp +++ b/motion/include/motion/navigation/NavigateThrough.hpp @@ -51,7 +51,7 @@ class NavigateThrough : public motion::BtActionNode< BT::InputPort("tf_frame"), BT::InputPort("style"), // straight, circular-l, circular-r BT::InputPort("n_poses"), - // BT::InputPort("radius"), + BT::InputPort("k_radius"), BT::InputPort("x"), BT::InputPort("y"), }); diff --git a/motion/src/motion/navigation/NavigateThrough.cpp b/motion/src/motion/navigation/NavigateThrough.cpp index db4da8d..1a8ca44 100644 --- a/motion/src/motion/navigation/NavigateThrough.cpp +++ b/motion/src/motion/navigation/NavigateThrough.cpp @@ -119,7 +119,6 @@ NavigateThrough::on_cancelled() return BT::NodeStatus::SUCCESS; } - std::vector NavigateThrough::generate_poses_to_goal( const geometry_msgs::msg::PoseStamped & goal, @@ -130,27 +129,40 @@ NavigateThrough::generate_poses_to_goal( { 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(); - // Fracción del camino (0.0 a 1.0) + // Fraction of the way (0.0 to 1.0) double fraction = static_cast(i) / (n_poses - 1); if (style == "straight") { - // Interpolación lineal directa entre robot y goal + // Linear interpolation pose.pose.position.x = map_to_robot.transform.translation.x + - fraction * (map_to_goal.transform.translation.x - map_to_robot.transform.translation.x); + fraction * dx; pose.pose.position.y = map_to_robot.transform.translation.y + - fraction * (map_to_goal.transform.translation.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") { - double radius = 1.0; - // getInput("radius", radius); - double angle = fraction * M_PI; // Desde 0 hasta 180 grados + 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; @@ -161,10 +173,10 @@ NavigateThrough::generate_poses_to_goal( 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; // Mantén z constante + pose.pose.position.z = map_to_robot.transform.translation.z; } - // Mantén la orientación del objetivo + // Same orientation as goal pose.pose.orientation = goal.pose.orientation; RCLCPP_INFO( @@ -178,7 +190,6 @@ NavigateThrough::generate_poses_to_goal( return poses; } - } // namespace navigation #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/perception/README.md b/perception/README.md index cff43c2..c5a8c58 100644 --- a/perception/README.md +++ b/perception/README.md @@ -1,2 +1,2 @@ -# perception_bt_nodes_irl +# perception_bt_nodes A collection of BT nodes for perception capabilities From c93136cc86b5deddabb087eef1e94e77097c4ab3 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 21 Nov 2024 14:28:38 +0100 Subject: [PATCH 10/60] enhancements --- hri/CMakeLists.txt | 7 +- hri/src/{hri => }/main_hri.cpp | 0 motion/CMakeLists.txt | 33 +++---- motion/bt_xml/spin.xml | 19 ++++ motion/bt_xml/spin_time.xml | 18 ++++ motion/config/{nav.yaml => motion.yaml} | 3 +- .../motion/base/{Rotate.hpp => Spin.hpp} | 11 ++- .../include/motion/navigation/NavigateTo.hpp | 1 - .../{nav.launch.py => motion.launch.py} | 4 +- motion/package.xml | 7 +- .../{motion/main_nav.cpp => main_motion.cpp} | 0 motion/src/motion/base/Rotate.cpp | 69 --------------- motion/src/motion/base/Spin.cpp | 86 +++++++++++++++++++ motion/src/motion/navigation/NavigateTo.cpp | 16 ++-- perception/CMakeLists.txt | 19 ++-- perception/bt_xml/detect_chair.xml | 50 +++++++++++ perception/bt_xml/indentify.xml | 48 +++++++++++ perception/config/perception.yaml | 9 ++ .../include/perception/IdentifyPerson.hpp | 6 +- perception/include/perception/IsDetected.hpp | 5 +- perception/launch/perception.launch.py | 50 +++++++++++ perception/package.xml | 1 + perception/src/main_perception.cpp | 75 ++++++++++++++++ perception/src/perception/IdentifyPerson.cpp | 56 ++++++++---- perception/src/perception/IsDetected.cpp | 45 ++++++---- 25 files changed, 481 insertions(+), 157 deletions(-) rename hri/src/{hri => }/main_hri.cpp (100%) create mode 100644 motion/bt_xml/spin.xml create mode 100644 motion/bt_xml/spin_time.xml rename motion/config/{nav.yaml => motion.yaml} (73%) rename motion/include/motion/base/{Rotate.hpp => Spin.hpp} (85%) rename motion/launch/{nav.launch.py => motion.launch.py} (93%) rename motion/src/{motion/main_nav.cpp => main_motion.cpp} (100%) delete mode 100644 motion/src/motion/base/Rotate.cpp create mode 100644 motion/src/motion/base/Spin.cpp create mode 100644 perception/bt_xml/detect_chair.xml create mode 100644 perception/bt_xml/indentify.xml create mode 100644 perception/config/perception.yaml create mode 100644 perception/launch/perception.launch.py create mode 100644 perception/src/main_perception.cpp diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt index 54c7d8e..2ed49d2 100644 --- a/hri/CMakeLists.txt +++ b/hri/CMakeLists.txt @@ -64,7 +64,7 @@ foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() -add_executable(hri_test src/hri/main_hri.cpp) +add_executable(hri_test src/main_hri.cpp) ament_target_dependencies(hri_test ${dependencies}) target_link_libraries(hri_test ${ZMQ_LIBRARIES}) @@ -90,6 +90,9 @@ endif() ament_export_include_directories(include) ament_export_dependencies(${dependencies}) -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +foreach(bt_plugin ${plugin_libs}) + ament_export_libraries(${bt_plugin}) +endforeach() ament_package() diff --git a/hri/src/hri/main_hri.cpp b/hri/src/main_hri.cpp similarity index 100% rename from hri/src/hri/main_hri.cpp rename to hri/src/main_hri.cpp diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt index d051e2b..f7c908f 100644 --- a/motion/CMakeLists.txt +++ b/motion/CMakeLists.txt @@ -15,13 +15,8 @@ find_package(nav2_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) -# find_package(nav2_costmap_2d REQUIRED) -# find_package(pluginlib REQUIRED) -find_package(navigation_system_interfaces REQUIRED) -# find_package(slam_toolbox REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(trajectory_msgs REQUIRED) -# find_package(lifecycle_msgs REQUIRED) find_package(trajectory_msgs REQUIRED) find_package(sensor_msgs REQUIRED) @@ -37,13 +32,8 @@ set(dependencies tf2_ros tf2_geometry_msgs tf2 - # nav2_costmap_2d - # pluginlib - navigation_system_interfaces - # slam_toolbox ament_index_cpp trajectory_msgs - # lifecycle_msgs sensor_msgs ) @@ -51,14 +41,14 @@ include_directories(include) add_library(navigate_to_bt_node SHARED src/motion/navigation/NavigateTo.cpp) add_library(navigate_through_bt_node SHARED src/motion/navigation/NavigateThrough.cpp) -add_library(rotate_bt_node SHARED src/motion/base/Rotate.cpp) +add_library(spin_bt_node SHARED src/motion/base/Spin.cpp) add_library(pan_bt_node SHARED src/motion/head/Pan.cpp) list(APPEND plugin_libs navigate_to_bt_node navigate_through_bt_node - rotate_bt_node + spin_bt_node pan_bt_node ) @@ -67,12 +57,12 @@ foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() -add_executable(nav_test src/motion/main_nav.cpp) -ament_target_dependencies(nav_test ${dependencies}) -target_link_libraries(nav_test ${ZMQ_LIBRARIES}) +add_executable(motion_test src/main_motion.cpp) +ament_target_dependencies(motion_test ${dependencies}) +target_link_libraries(motion_test ${ZMQ_LIBRARIES}) install(TARGETS - nav_test + motion_test ${plugin_libs} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib @@ -84,10 +74,6 @@ install(DIRECTORY include launch config bt_xml DESTINATION share/${PROJECT_NAME} ) -# install(FILES clear_people_layer.xml -# DESTINATION share/${PROJECT_NAME} -# ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) @@ -96,9 +82,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -# ament_export_libraries(clear_people_layer) ament_export_dependencies(${dependencies}) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -# pluginlib_export_plugin_description_file(nav2_costmap_2d clear_people_layer.xml) +foreach(bt_plugin ${plugin_libs}) + ament_export_libraries(${bt_plugin}) +endforeach() + ament_package() diff --git a/motion/bt_xml/spin.xml b/motion/bt_xml/spin.xml new file mode 100644 index 0000000..4b1ca17 --- /dev/null +++ b/motion/bt_xml/spin.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + 1: left; -1: right + + + + + diff --git a/motion/bt_xml/spin_time.xml b/motion/bt_xml/spin_time.xml new file mode 100644 index 0000000..1c7b162 --- /dev/null +++ b/motion/bt_xml/spin_time.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + 1: left; -1: right + + + + + diff --git a/motion/config/nav.yaml b/motion/config/motion.yaml similarity index 73% rename from motion/config/nav.yaml rename to motion/config/motion.yaml index f8cb26f..9058a5a 100644 --- a/motion/config/nav.yaml +++ b/motion/config/motion.yaml @@ -1,7 +1,8 @@ nav_node: ros__parameters: use_sim_time: True - bt_xml_file: nav_through.xml + bt_xml_file: spin.xml plugins: - navigate_to_bt_node - navigate_through_bt_node + - spin_bt_node diff --git a/motion/include/motion/base/Rotate.hpp b/motion/include/motion/base/Spin.hpp similarity index 85% rename from motion/include/motion/base/Rotate.hpp rename to motion/include/motion/base/Spin.hpp index d2e41b4..d2696cb 100644 --- a/motion/include/motion/base/Rotate.hpp +++ b/motion/include/motion/base/Spin.hpp @@ -34,10 +34,10 @@ namespace base { -class Rotate : public BT::ActionNodeBase +class Spin : public BT::ActionNodeBase { public: - explicit Rotate(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); + explicit Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt(); BT::NodeStatus tick(); @@ -47,7 +47,8 @@ class Rotate : public BT::ActionNodeBase return BT::PortsList( { BT::InputPort("angle"), // if -1, it will spin forever - BT::InputPort("speed") + BT::InputPort("speed"), + BT::InputPort("direction") // 1: left, -1: right }); } @@ -57,10 +58,12 @@ class Rotate : public BT::ActionNodeBase rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; double angle_, rotated_angle_, speed_; + + int direction_; std::chrono::time_point last_time_; }; -} // namespace navigation +} // namespace base #endif // NAVIGATION__ROTATE_HPP_ diff --git a/motion/include/motion/navigation/NavigateTo.hpp b/motion/include/motion/navigation/NavigateTo.hpp index 5b02031..1e9e849 100644 --- a/motion/include/motion/navigation/NavigateTo.hpp +++ b/motion/include/motion/navigation/NavigateTo.hpp @@ -28,7 +28,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "motion/navigation/utils.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" -#include "navigation_system_interfaces/srv/set_truncate_distance.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" diff --git a/motion/launch/nav.launch.py b/motion/launch/motion.launch.py similarity index 93% rename from motion/launch/nav.launch.py rename to motion/launch/motion.launch.py index 0c7707b..7a58431 100644 --- a/motion/launch/nav.launch.py +++ b/motion/launch/motion.launch.py @@ -25,7 +25,7 @@ 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', 'nav.yaml') + 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'] @@ -35,7 +35,7 @@ def generate_launch_description(): nav_cmd = Node( package='motion_bt_nodes', - executable='nav_test', + executable='motion_test', output='screen', remappings=[ ], diff --git a/motion/package.xml b/motion/package.xml index e3c7712..9ae3295 100644 --- a/motion/package.xml +++ b/motion/package.xml @@ -16,10 +16,11 @@ behaviortree_cpp_v3 geometry_msgs nav2_msgs - nav2_costmap_2d navigation_system_interfaces - slam_toolbox - + tf2_ros + tf2_geometry_msgs + tf2 + ament_lint_auto ament_lint_common diff --git a/motion/src/motion/main_nav.cpp b/motion/src/main_motion.cpp similarity index 100% rename from motion/src/motion/main_nav.cpp rename to motion/src/main_motion.cpp diff --git a/motion/src/motion/base/Rotate.cpp b/motion/src/motion/base/Rotate.cpp deleted file mode 100644 index a156db9..0000000 --- a/motion/src/motion/base/Rotate.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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/Rotate.hpp" - -namespace base -{ - -Rotate::Rotate(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 Rotate::tick() -{ - RCLCPP_INFO(node_->get_logger(), "Rotate ticked"); - - getInput("angle", angle_); - getInput("speed", speed_); - - if (status() == BT::NodeStatus::IDLE) { - rotated_angle_ = 0; - last_time_ = std::chrono::high_resolution_clock::now(); - return BT::NodeStatus::RUNNING; - } - - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.angular.z = speed_; - - if (angle_ < 0) { - vel_pub_->publish(cmd_vel); - return BT::NodeStatus::RUNNING; - } else if ((rotated_angle_ < angle_) && (angle_ >= 0)) { - 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; - last_time_ = curr_time; - return BT::NodeStatus::RUNNING; - } - - return BT::NodeStatus::FAILURE; -} - -void Rotate::halt() -{ - RCLCPP_INFO(node_->get_logger(), "Rotate halted"); -} - -} // namespace navigation - -BT_REGISTER_NODES(factory) { - factory.registerNodeType("Rotate"); -} diff --git a/motion/src/motion/base/Spin.cpp b/motion/src/motion/base/Spin.cpp new file mode 100644 index 0000000..3d164d4 --- /dev/null +++ b/motion/src/motion/base/Spin.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 "motion/base/Spin.hpp" + +namespace base +{ + +Spin::Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf), + direction_(-1) +{ + config().blackboard->get("node", node_); + + getInput("angle", angle_); + getInput("speed", speed_); + getInput("direction", direction_); + + if (angle_ < 0) { + RCLCPP_INFO(node_->get_logger(), "Spinning forever"); + } else { + RCLCPP_INFO(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s (dir: %d)", angle_, speed_, direction_); + } + + vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_->on_activate(); +} + +BT::NodeStatus Spin::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "Spin ticked"); + + 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_; + + if (angle_ < 0) { + RCLCPP_DEBUG(node_->get_logger(), "Spinning forever"); + vel_pub_->publish(cmd_vel); + return BT::NodeStatus::RUNNING; + } else if ((rotated_angle_ < angle_) && (angle_ >= 0)) { + RCLCPP_DEBUG(node_->get_logger(), "Target: %.2f degrees (dir: %d). Spinned %.2f degrees: ", angle_, direction_, 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/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index 323877e..b15cf5c 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -51,13 +51,15 @@ void NavigateTo::on_tick() 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; - + // 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; + + goal.header.frame_id = tf_frame; + goal.pose.position.z = 0.0; } else { // No TF, use coordinates getInput("x", goal.pose.position.x); diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index 9b20ec6..edb7f01 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 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) @@ -23,12 +24,11 @@ find_package(OpenCV REQUIRED) find_package(perception_system_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) -# find_package(backward_ros REQUIRED) - set(CMAKE_CXX_STANDARD 17) set(dependencies + ament_index_cpp rclcpp rclcpp_cascade_lifecycle rclpy @@ -45,12 +45,10 @@ set(dependencies perception_system_interfaces sensor_msgs std_srvs - # backward_ros ) include_directories(include) - add_library(is_detected_bt_node SHARED src/perception/IsDetected.cpp) add_library(filter_entity_bt_node SHARED src/perception/FilterEntity.cpp) add_library(is_pointing_bt_node SHARED src/perception/IsPointing.cpp) @@ -68,7 +66,12 @@ foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() +add_executable(perception_test src/main_perception.cpp) +ament_target_dependencies(perception_test ${dependencies}) +target_link_libraries(perception_test ${ZMQ_LIBRARIES}) + install(TARGETS + perception_test ${plugin_libs} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib @@ -76,8 +79,8 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ - DESTINATION include/ +install(DIRECTORY include launch config bt_xml + DESTINATION share/${PROJECT_NAME} ) install(PROGRAMS @@ -94,4 +97,8 @@ endif() ament_export_dependencies(${dependencies}) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +foreach(bt_plugin ${plugin_libs}) + ament_export_libraries(${bt_plugin}) +endforeach() + ament_package() diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml new file mode 100644 index 0000000..a5580c8 --- /dev/null +++ b/perception/bt_xml/detect_chair.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + Where to navigate + False in case it is navigating to follow something + + + + + + 1: left; -1: right + + + + + diff --git a/perception/bt_xml/indentify.xml b/perception/bt_xml/indentify.xml new file mode 100644 index 0000000..c23cca2 --- /dev/null +++ b/perception/bt_xml/indentify.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + True in case we do not want to reach the destination but to stay close + Where to navigate + False in case it is navigating to follow something + + + + 1: left; -1: right + + + + + diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml new file mode 100644 index 0000000..29d04af --- /dev/null +++ b/perception/config/perception.yaml @@ -0,0 +1,9 @@ +perception_node: + ros__parameters: + use_sim_time: True + bt_xml_file: detect_chair.xml + plugins: + - is_detected_bt_node + - identify_person_bt_node + - spin_bt_node + - navigate_to_bt_node diff --git a/perception/include/perception/IdentifyPerson.hpp b/perception/include/perception/IdentifyPerson.hpp index ee3100c..5630c70 100644 --- a/perception/include/perception/IdentifyPerson.hpp +++ b/perception/include/perception/IdentifyPerson.hpp @@ -50,7 +50,8 @@ class IdentifyPerson : public BT::ActionNodeBase return BT::PortsList( { BT::InputPort("person_to_identify"), - BT::InputPort("get_features") // If true, features are saved. If false, features were already saved + BT::InputPort("get_features"), // If true, features are saved. If false, features were already saved + BT::InputPort("confidence") }); } @@ -61,8 +62,7 @@ class IdentifyPerson : public BT::ActionNodeBase std::string person_; bool get_features_; - // tf2::BufferCore tf_buffer_; - // tf2_ros::TransformListener tf_listener_; + float confidence_; }; diff --git a/perception/include/perception/IsDetected.hpp b/perception/include/perception/IsDetected.hpp index 8814a58..117b40d 100644 --- a/perception/include/perception/IsDetected.hpp +++ b/perception/include/perception/IsDetected.hpp @@ -50,13 +50,14 @@ class IsDetected : public BT::ConditionNode return BT::PortsList( { BT::InputPort("max_entities"), - BT::InputPort("person_id"), + // BT::InputPort("person_id"), + BT::InputPort("what"), BT::InputPort("cam_frame"), BT::InputPort("interest"), BT::InputPort("confidence"), BT::InputPort("order"), BT::InputPort("max_depth"), - BT::OutputPort>("frames"), // TF frames of the detected entities + BT::OutputPort("frame"), // TF frame of the detected entity BT::OutputPort("best_detection") }); } 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 index 18ac62f..dc4705c 100644 --- a/perception/package.xml +++ b/perception/package.xml @@ -27,6 +27,7 @@ rclcpp_action perception_system perception_system_interfaces + motion_bt_nodes sensor_msgs ament_lint_auto diff --git a/perception/src/main_perception.cpp b/perception/src/main_perception.cpp new file mode 100644 index 0000000..f698f1d --- /dev/null +++ b/perception/src/main_perception.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("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->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(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/perception/src/perception/IdentifyPerson.cpp b/perception/src/perception/IdentifyPerson.cpp index 78901dd..c40af82 100644 --- a/perception/src/perception/IdentifyPerson.cpp +++ b/perception/src/perception/IdentifyPerson.cpp @@ -24,40 +24,61 @@ IdentifyPerson::IdentifyPerson( const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), - person_("") - // get_features_(false), - // tf_buffer_(), - // tf_listener_(tf_buffer_) + person_(""), + get_features_(false), + confidence_(0.5) { config().blackboard->get("node", node_); + getInput("person_to_identify", person_); getInput("get_features", get_features_); + static_broadcaster_ = std::make_shared(node_); } BT::NodeStatus IdentifyPerson::tick() { + std::vector detections; + try { - // // Get the transform from map to the person - // auto map2person_msg = tf_buffer_.lookupTransform("map", person_, tf2::TimePointZero); - // tf2::Stamped map2person; - // tf2::fromMsg(map2person_msg, map2person); - - // // Publish transform - // static_broadcaster_->sendTransform(tf2::toMsg(map2person)); if (get_features_) // Configure perception system to track the specified person { - // pl::getInstance(node_)->set_features_of_interest(person_); + RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", person_.c_str()); + detections = pl::getInstance(node_)->get_by_type("person"); + + std::sort( + detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + return a.center3d.position.z < b.center3d.position.z; + }); + + // detections = pl::getInstance(node_)->set_features_of_interest(detections[0]); + config().blackboard->set(person_, detections[0]); + } + + RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", person_.c_str()); + perception_system_interfaces::msg::Detection person_detection; + config().blackboard->get(person_, person_detection); + detections = pl::getInstance(node_)->get_by_features(person_detection, confidence_); + + std::sort( + detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + return a.score > b.score; + }); + + if (detections.empty()) + { + RCLCPP_WARN(node_->get_logger(), "%s NOT detected", person_.c_str()); + return BT::NodeStatus::FAILURE; } - // Get the detection of the person with the previously configured features - perception_system_interfaces::msg::Detection detection; - // detection = pl::getInstance(node_)->get_by_features(person_); + + RCLCPP_DEBUG(node_->get_logger(), "%s detected with confidence %f", person_.c_str(), detections[0].score); + // Publish the detection - pl::getInstance(node_)->publicTF(detection, person_); - + pl::getInstance(node_)->publishTF(detections[0], person_); + return BT::NodeStatus::SUCCESS; } catch(const std::exception& ex) @@ -69,7 +90,6 @@ IdentifyPerson::tick() return BT::NodeStatus::RUNNING; } - } // namespace perception BT_REGISTER_NODES(factory) { diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/IsDetected.cpp index ff256f1..61dda1d 100644 --- a/perception/src/perception/IsDetected.cpp +++ b/perception/src/perception/IsDetected.cpp @@ -25,9 +25,19 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura max_depth_(std::numeric_limits::max()), max_entities_(1) { + std::string what; config().blackboard->get("node", node_); - - // node_->add_activation("perception_system/perception_people_detection") + config().blackboard->get("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"); + } + getInput("interest", interest_); getInput("cam_frame", cam_frame_); @@ -35,19 +45,23 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura getInput("max_entities", max_entities_); getInput("order", order_); getInput("max_depth", max_depth_); - getInput("person_id", person_id_); + // getInput("person_id", person_id_); + + RCLCPP_INFO(node_->get_logger(), "Interest: %s", interest_.c_str()); + + frames_.clear(); } BT::NodeStatus IsDetected::tick() { rclcpp::spin_some(node_->get_node_base_interface()); - getInput("person_id", person_id_); if (status() == BT::NodeStatus::IDLE) { - RCLCPP_INFO(node_->get_logger(), "IsDetected ticked while IDLE"); + RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); } RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked"); + pl::getInstance(node_)->set_interest(interest_, true); pl::getInstance(node_)->update(35); @@ -58,7 +72,7 @@ BT::NodeStatus IsDetected::tick() return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Processing %ld detections...", detections.size()); + RCLCPP_INFO(node_->get_logger(), "[IsDetected] 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 @@ -80,15 +94,12 @@ BT::NodeStatus IsDetected::tick() // pub->publish(detections[0].image); - setOutput("best_detection", detections[0].class_name); - RCLCPP_INFO(node_->get_logger(), "[IsDetected] Detections sorted"); RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Max Depth: %f", max_depth_); RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] 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(), "[IsDetected] Removing detection %s", detection.class_name.c_str()); @@ -98,24 +109,26 @@ BT::NodeStatus IsDetected::tick() } else { frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter)); - if (pl::getInstance(node_)->publicTF(detection, std::to_string(entity_counter)) == -1) { + if (pl::getInstance(node_)->publishTF(detection, std::to_string(entity_counter)) == -1) { return BT::NodeStatus::FAILURE; } ++it; ++entity_counter; } } - + RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections sorted and filtered"); if (frames_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "[IsDetected] No detections after filter"); + RCLCPP_ERROR(node_->get_logger(), "[IsDetected] No detections after filtering"); return BT::NodeStatus::FAILURE; } + + RCLCPP_INFO(node_->get_logger(), "[IsDetected] A %s has been found with the provided conditions", detections[0].class_name.c_str()); + setOutput("best_detection", detections[0].class_name); + // setOutput("frames", frames_); + setOutput("frame", frames_[0]); - setOutput("frames", frames_); - frames_.clear(); - - RCLCPP_DEBUG(node_->get_logger(), "Pointing direction: %d", detections[0].pointing_direction); + // frames_.clear(); RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections published"); return BT::NodeStatus::SUCCESS; From 8b951c6f9735024096a15aec5a34b0d524d0d687 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 21 Nov 2024 23:14:18 +0100 Subject: [PATCH 11/60] additions to test --- hri/CMakeLists.txt | 2 +- hri/include/hri/dialog/DialogConfirmation.hpp | 19 +++- hri/src/hri/dialog/DialogConfirmation.cpp | 26 ++--- motion/CMakeLists.txt | 2 + motion/bt_xml/face.xml | 31 ++++++ motion/bt_xml/face_spin.xml | 36 +++++++ motion/bt_xml/spin.xml | 6 +- motion/bt_xml/spin_time.xml | 4 +- motion/config/motion.yaml | 1 + motion/include/motion/base/Face.hpp | 73 ++++++++++++++ motion/include/motion/base/Spin.hpp | 13 +-- motion/src/main_motion.cpp | 2 + motion/src/motion/base/Face.cpp | 99 +++++++++++++++++++ motion/src/motion/base/Spin.cpp | 35 ++++--- perception/CMakeLists.txt | 4 + perception/bt_xml/detect_chair.xml | 7 +- perception/bt_xml/in_front.xml | 60 +++++++++++ perception/bt_xml/indentify.xml | 11 ++- perception/include/perception/GetAngle.hpp | 71 +++++++++++++ .../include/perception/IdentifyPerson.hpp | 3 +- perception/include/perception/IsDetected.hpp | 4 +- perception/include/perception/IsInFront.hpp | 68 +++++++++++++ perception/include/perception/IsPointing.hpp | 4 +- perception/src/perception/GetAngle.cpp | 86 ++++++++++++++++ perception/src/perception/IdentifyPerson.cpp | 19 ++-- perception/src/perception/IsDetected.cpp | 34 ++++--- perception/src/perception/IsInFront.cpp | 86 ++++++++++++++++ perception/src/perception/IsPointing.cpp | 36 +++++-- 28 files changed, 752 insertions(+), 90 deletions(-) create mode 100644 motion/bt_xml/face.xml create mode 100644 motion/bt_xml/face_spin.xml create mode 100644 motion/include/motion/base/Face.hpp create mode 100644 motion/src/motion/base/Face.cpp create mode 100644 perception/bt_xml/in_front.xml create mode 100644 perception/include/perception/GetAngle.hpp create mode 100644 perception/include/perception/IsInFront.hpp create mode 100644 perception/src/perception/GetAngle.cpp create mode 100644 perception/src/perception/IsInFront.cpp diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt index 2ed49d2..fa57994 100644 --- a/hri/CMakeLists.txt +++ b/hri/CMakeLists.txt @@ -49,7 +49,7 @@ 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/Speak.cpp) +add_library(dialog_confirmation_bt_node SHARED src/hri/dialog/DialogConfirmation.cpp) add_library(query_bt_node SHARED src/hri/dialog/Query.cpp) list(APPEND plugin_libs diff --git a/hri/include/hri/dialog/DialogConfirmation.hpp b/hri/include/hri/dialog/DialogConfirmation.hpp index 9ce9822..91ab0cc 100644 --- a/hri/include/hri/dialog/DialogConfirmation.hpp +++ b/hri/include/hri/dialog/DialogConfirmation.hpp @@ -17,20 +17,23 @@ #include #include +#include #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" -#include "hri/dialog/BTActionNode.hpp" +#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 +#include "std_msgs/msg/int8.hpp" + +namespace hri { class DialogConfirmation - : public dialog::BtActionNode< + : public hri::BtActionNode< whisper_msgs::action::STT, rclcpp_cascade_lifecycle::CascadeLifecycleNode> { public: @@ -41,10 +44,18 @@ class DialogConfirmation void on_tick() override; BT::NodeStatus on_success() override; - static BT::PortsList providedPorts() {return BT::PortsList({});} + 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}; }; diff --git a/hri/src/hri/dialog/DialogConfirmation.cpp b/hri/src/hri/dialog/DialogConfirmation.cpp index 27b5137..a4169a0 100644 --- a/hri/src/hri/dialog/DialogConfirmation.cpp +++ b/hri/src/hri/dialog/DialogConfirmation.cpp @@ -14,15 +14,9 @@ #include "hri/dialog/DialogConfirmation.hpp" -#include -#include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "hri/dialog/DialogConfirmation.hpp" -#include "std_msgs/msg/int8.hpp" -#include "whisper_msgs/action/stt.hpp" -namespace dialog +namespace hri { using namespace std::chrono_literals; @@ -31,10 +25,12 @@ using namespace std::placeholders; DialogConfirmation::DialogConfirmation( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: dialog::BtActionNode( +: hri::BtActionNode( xml_tag_name, action_name, conf) { speech_start_publisher_ = node_->create_publisher("dialog_action", 10); + + getInput("language", lang_); } void DialogConfirmation::on_tick() @@ -61,20 +57,26 @@ BT::NodeStatus DialogConfirmation::on_success() std::transform( result_.result->transcription.text.begin(), result_.result->transcription.text.end(), result_.result->transcription.text.begin(), [](unsigned char c) {return std::tolower(c);}); - if (result_.result->transcription.text.find("yes") != std::string::npos) { + + 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 dialog +} // 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); + return std::make_unique(name, "/whisper/listen", config); }; - factory.registerBuilder("DialogConfirmation", builder); + factory.registerBuilder("DialogConfirmation", builder); } diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt index f7c908f..c8b0de0 100644 --- a/motion/CMakeLists.txt +++ b/motion/CMakeLists.txt @@ -42,6 +42,7 @@ include_directories(include) add_library(navigate_to_bt_node SHARED src/motion/navigation/NavigateTo.cpp) add_library(navigate_through_bt_node SHARED src/motion/navigation/NavigateThrough.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 @@ -49,6 +50,7 @@ list(APPEND navigate_to_bt_node navigate_through_bt_node spin_bt_node + face_bt_node pan_bt_node ) 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/spin.xml b/motion/bt_xml/spin.xml index 4b1ca17..29b1c50 100644 --- a/motion/bt_xml/spin.xml +++ b/motion/bt_xml/spin.xml @@ -3,16 +3,16 @@ - - + + - 1: left; -1: right + diff --git a/motion/bt_xml/spin_time.xml b/motion/bt_xml/spin_time.xml index 1c7b162..90b8f45 100644 --- a/motion/bt_xml/spin_time.xml +++ b/motion/bt_xml/spin_time.xml @@ -3,15 +3,15 @@ - + - 1: left; -1: right + diff --git a/motion/config/motion.yaml b/motion/config/motion.yaml index 9058a5a..ed56dd8 100644 --- a/motion/config/motion.yaml +++ b/motion/config/motion.yaml @@ -6,3 +6,4 @@ nav_node: - navigate_to_bt_node - navigate_through_bt_node - spin_bt_node + - get_angle_bt_node 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 index d2696cb..750bdae 100644 --- a/motion/include/motion/base/Spin.hpp +++ b/motion/include/motion/base/Spin.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAVIGATION__ROTATE_HPP_ -#define NAVIGATION__ROTATE_HPP_ +#ifndef BASE_ROTATE_HPP_ +#define BASE_ROTATE_HPP_ #include @@ -48,7 +48,8 @@ class Spin : public BT::ActionNodeBase { BT::InputPort("angle"), // if -1, it will spin forever BT::InputPort("speed"), - BT::InputPort("direction") // 1: left, -1: right + // BT::InputPort("direction"), // 1: left, -1: right + BT::InputPort("forever") }); } @@ -58,12 +59,12 @@ class Spin : public BT::ActionNodeBase rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; double angle_, rotated_angle_, speed_; - - int direction_; + bool forever_; + // int direction_; std::chrono::time_point last_time_; }; } // namespace base -#endif // NAVIGATION__ROTATE_HPP_ +#endif // BASE_ROTATE_HPP_ diff --git a/motion/src/main_motion.cpp b/motion/src/main_motion.cpp index 8eeec64..6deffa8 100644 --- a/motion/src/main_motion.cpp +++ b/motion/src/main_motion.cpp @@ -68,6 +68,8 @@ int main(int argc, char * argv[]) rate.sleep(); } + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + RCLCPP_INFO(node->get_logger(), "Finished behavior from BT"); rclcpp::shutdown(); diff --git a/motion/src/motion/base/Face.cpp b/motion/src/motion/base/Face.cpp new file mode 100644 index 0000000..56b63d9 --- /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 index 3d164d4..2904f69 100644 --- a/motion/src/motion/base/Spin.cpp +++ b/motion/src/motion/base/Spin.cpp @@ -18,21 +18,10 @@ namespace base { Spin::Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) -: BT::ActionNodeBase(xml_tag_name, conf), - direction_(-1) +: BT::ActionNodeBase(xml_tag_name, conf) { config().blackboard->get("node", node_); - getInput("angle", angle_); - getInput("speed", speed_); - getInput("direction", direction_); - - if (angle_ < 0) { - RCLCPP_INFO(node_->get_logger(), "Spinning forever"); - } else { - RCLCPP_INFO(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s (dir: %d)", angle_, speed_, direction_); - } - vel_pub_ = node_->create_publisher("cmd_vel", 1); vel_pub_->on_activate(); } @@ -41,6 +30,18 @@ BT::NodeStatus Spin::tick() { RCLCPP_DEBUG(node_->get_logger(), "Spin ticked"); + getInput("angle", angle_); + getInput("speed", speed_); + getInput("forever", forever_); + // getInput("direction", direction_); + + if (forever_) { + RCLCPP_DEBUG(node_->get_logger(), "Spinning forever"); + } else { + // RCLCPP_INFO(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s (dir: %d)", angle_, speed_, direction_); + RCLCPP_DEBUG(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(); @@ -51,14 +52,16 @@ BT::NodeStatus Spin::tick() } geometry_msgs::msg::Twist cmd_vel; - cmd_vel.angular.z = speed_ * direction_; + // cmd_vel.angular.z = speed_ * direction_; + cmd_vel.angular.z = speed_ * std::copysign(1, angle_); - if (angle_ < 0) { + if (forever_) { RCLCPP_DEBUG(node_->get_logger(), "Spinning forever"); vel_pub_->publish(cmd_vel); return BT::NodeStatus::RUNNING; - } else if ((rotated_angle_ < angle_) && (angle_ >= 0)) { - RCLCPP_DEBUG(node_->get_logger(), "Target: %.2f degrees (dir: %d). Spinned %.2f degrees: ", angle_, direction_, rotated_angle_); + } 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_); diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index edb7f01..09ff908 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -53,12 +53,16 @@ add_library(is_detected_bt_node SHARED src/perception/IsDetected.cpp) add_library(filter_entity_bt_node SHARED src/perception/FilterEntity.cpp) add_library(is_pointing_bt_node SHARED src/perception/IsPointing.cpp) add_library(identify_person_bt_node SHARED src/perception/IdentifyPerson.cpp) +add_library(get_angle_bt_node SHARED src/perception/GetAngle.cpp) +add_library(is_in_front_bt_node SHARED src/perception/IsInFront.cpp) list(APPEND plugin_libs is_detected_bt_node filter_entity_bt_node is_pointing_bt_node identify_person_bt_node + get_angle_bt_node + is_in_front_bt_node ) foreach(bt_plugin ${plugin_libs}) diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml index a5580c8..64df3f2 100644 --- a/perception/bt_xml/detect_chair.xml +++ b/perception/bt_xml/detect_chair.xml @@ -4,10 +4,10 @@ - + - + @@ -17,9 +17,10 @@ - + + diff --git a/perception/bt_xml/in_front.xml b/perception/bt_xml/in_front.xml new file mode 100644 index 0000000..b0582c9 --- /dev/null +++ b/perception/bt_xml/in_front.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + How far from the target + True in case we do not want to reach the destination but to stay close + Where to navigate + False in case it is navigating to follow something + + + + + + + + + diff --git a/perception/bt_xml/indentify.xml b/perception/bt_xml/indentify.xml index c23cca2..fba076b 100644 --- a/perception/bt_xml/indentify.xml +++ b/perception/bt_xml/indentify.xml @@ -3,23 +3,26 @@ - + + - + + - + - + + diff --git a/perception/include/perception/GetAngle.hpp b/perception/include/perception/GetAngle.hpp new file mode 100644 index 0000000..b2d1112 --- /dev/null +++ b/perception/include/perception/GetAngle.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/IdentifyPerson.hpp b/perception/include/perception/IdentifyPerson.hpp index 5630c70..a167dd0 100644 --- a/perception/include/perception/IdentifyPerson.hpp +++ b/perception/include/perception/IdentifyPerson.hpp @@ -51,7 +51,8 @@ class IdentifyPerson : public BT::ActionNodeBase { BT::InputPort("person_to_identify"), BT::InputPort("get_features"), // If true, features are saved. If false, features were already saved - BT::InputPort("confidence") + BT::InputPort("confidence"), + BT::InputPort("detection"), }); } diff --git a/perception/include/perception/IsDetected.hpp b/perception/include/perception/IsDetected.hpp index 117b40d..a77a2ef 100644 --- a/perception/include/perception/IsDetected.hpp +++ b/perception/include/perception/IsDetected.hpp @@ -58,7 +58,9 @@ class IsDetected : public BT::ConditionNode BT::InputPort("order"), BT::InputPort("max_depth"), BT::OutputPort("frame"), // TF frame of the detected entity - BT::OutputPort("best_detection") + // BT::OutputPort("best_detection"), + BT::OutputPort("detection"), + BT::OutputPort("n_detections"), }); } diff --git a/perception/include/perception/IsInFront.hpp b/perception/include/perception/IsInFront.hpp new file mode 100644 index 0000000..1ecca21 --- /dev/null +++ b/perception/include/perception/IsInFront.hpp @@ -0,0 +1,68 @@ +// 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 + +#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 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("confidence"), + BT::InputPort("target"), + BT::InputPort("what"), + BT::OutputPort("direction") + }); + } + +private: + std::shared_ptr node_; + + std::string target_; + double confidence_; + +}; + +} // namespace perception + +#endif // PERCEPTION_IS_IN_FRONT_HPP_ diff --git a/perception/include/perception/IsPointing.hpp b/perception/include/perception/IsPointing.hpp index f50dd83..22f1bba 100644 --- a/perception/include/perception/IsPointing.hpp +++ b/perception/include/perception/IsPointing.hpp @@ -54,9 +54,11 @@ class IsPointing : public BT::ConditionNode BT::InputPort("camera_frame"), BT::InputPort("low_pointing_limit"), BT::InputPort("high_pointing_limit"), + BT::InputPort("output_frame"), BT::OutputPort("output_frame"), BT::OutputPort("pointing_direction"), - BT::OutputPort("person_id") + // BT::OutputPort("person_id"), + BT::OutputPort("detection") }); } diff --git a/perception/src/perception/GetAngle.cpp b/perception/src/perception/GetAngle.cpp new file mode 100644 index 0000000..33bf285 --- /dev/null +++ b/perception/src/perception/GetAngle.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/GetAngle.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"); + + std::vector detections; + perception_system_interfaces::msg::Detection detection; + + config().blackboard->get(target_, detection); + + rclcpp::spin_some(node_->get_node_base_interface()); + + 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/IdentifyPerson.cpp b/perception/src/perception/IdentifyPerson.cpp index c40af82..77175cb 100644 --- a/perception/src/perception/IdentifyPerson.cpp +++ b/perception/src/perception/IdentifyPerson.cpp @@ -40,26 +40,25 @@ BT::NodeStatus IdentifyPerson::tick() { std::vector detections; - + perception_system_interfaces::msg::Detection person_detection; try { if (get_features_) // Configure perception system to track the specified person { RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", person_.c_str()); - detections = pl::getInstance(node_)->get_by_type("person"); - - std::sort( - detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - return a.center3d.position.z < b.center3d.position.z; - }); - - // detections = pl::getInstance(node_)->set_features_of_interest(detections[0]); + // detections = pl::getInstance(node_)->get_by_type("person"); + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return a.center3d.position.z < b.center3d.position.z; + // }); + getInput("detection", person_detection); + // detections = pl::getInstance(node_)->set_features_of_interest(person_detection); config().blackboard->set(person_, detections[0]); } RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", person_.c_str()); - perception_system_interfaces::msg::Detection person_detection; + config().blackboard->get(person_, person_detection); detections = pl::getInstance(node_)->get_by_features(person_detection, confidence_); diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/IsDetected.cpp index 61dda1d..e80e58f 100644 --- a/perception/src/perception/IsDetected.cpp +++ b/perception/src/perception/IsDetected.cpp @@ -34,7 +34,7 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura } 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()); + RCLCPP_ERROR(node_->get_logger(), "Unknown what: %s. Activating generic", what.c_str()); node_->add_activation("perception_system/perception_object_detection"); } @@ -68,22 +68,22 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - RCLCPP_WARN(node_->get_logger(), "[IsDetected] No detections"); + RCLCPP_WARN(node_->get_logger(), "No detections"); return BT::NodeStatus::FAILURE; } - RCLCPP_INFO(node_->get_logger(), "[IsDetected] Processing %ld detections...", detections.size()); + RCLCPP_INFO(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(), "[IsDetected] Sorting detections by color"); + // 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(), "[IsDetected] Sorting detections by 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; @@ -95,20 +95,20 @@ BT::NodeStatus IsDetected::tick() // pub->publish(detections[0].image); - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Max Depth: %f", max_depth_); - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Threshold: %f", threshold_); + 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(), "[IsDetected] Removing detection %s", detection.class_name.c_str()); - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Score: %f", detection.score); - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Depth: %f", detection.center3d.position.z); + 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 { - frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter)); + frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter + 1)); if (pl::getInstance(node_)->publishTF(detection, std::to_string(entity_counter)) == -1) { return BT::NodeStatus::FAILURE; } @@ -117,20 +117,22 @@ BT::NodeStatus IsDetected::tick() } } - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections sorted and filtered"); + RCLCPP_DEBUG(node_->get_logger(), "Detections sorted and filtered"); if (frames_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "[IsDetected] No detections after filtering"); + RCLCPP_ERROR(node_->get_logger(), "No detections after filtering"); return BT::NodeStatus::FAILURE; } - RCLCPP_INFO(node_->get_logger(), "[IsDetected] A %s has been found with the provided conditions", detections[0].class_name.c_str()); - setOutput("best_detection", detections[0].class_name); + RCLCPP_INFO(node_->get_logger(), "A %s has been found with the provided conditions", detections[0].class_name.c_str()); + // setOutput("best_detection", detections[0].class_name); + setOutput("detection", detections[0]); + setOutput("n_dectections", static_cast(detections.size()) + 1); // setOutput("frames", frames_); setOutput("frame", frames_[0]); // frames_.clear(); - RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections published"); + RCLCPP_DEBUG(node_->get_logger(), "Detections published"); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/IsInFront.cpp b/perception/src/perception/IsInFront.cpp new file mode 100644 index 0000000..83365eb --- /dev/null +++ b/perception/src/perception/IsInFront.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/IsInFront.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) +{ + std::string what; + + config().blackboard->get("node", node_); + 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(), "Unknown what: %s. Activating generic", what.c_str()); + node_->add_activation("perception_system/perception_object_detection"); + } +} +BT::NodeStatus IsInFront::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IsInFront ticked"); + + std::vector detections; + perception_system_interfaces::msg::Detection detection; + + config().blackboard->get(target_, detection); + + rclcpp::spin_some(node_->get_node_base_interface()); + + detections = pl::getInstance(node_)->get_by_features(detection, confidence_); + + if (detections.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No detections found"); + setOutput("direction", -1); // If no detections, just turn right bu default + 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); + yaw = yaw * 180.0 / M_PI; + + if (std::abs((yaw)) > 5.0) { // If the angle is greater than 5 degrees, the detection is not in front + if (yaw > 0) { + setOutput("direction", 1); + } else { + setOutput("direction", -1); + } + return BT::NodeStatus::FAILURE; + } + + // The detection is in front + setOutput("direction", 0); + return BT::NodeStatus::SUCCESS; + + +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsInFront"); +} diff --git a/perception/src/perception/IsPointing.cpp b/perception/src/perception/IsPointing.cpp index fb1271d..0aca8e7 100644 --- a/perception/src/perception/IsPointing.cpp +++ b/perception/src/perception/IsPointing.cpp @@ -69,25 +69,25 @@ int IsPointing::publish_detection_tf( geometry_msgs::msg::TransformStamped map2camera_msg; RCLCPP_INFO( - node_->get_logger(), "[IsPointing] Detectection in frame_id %s", + node_->get_logger(), "Detectection in frame_id %s", detection.header.frame_id.c_str()); try { map2camera_msg = tf_buffer_->lookupTransform("map", camera_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { RCLCPP_INFO( - node_->get_logger(), "[IsPointing] Could not transform %s to %s: %s", "map", + node_->get_logger(), "Could not transform %s to %s: %s", "map", camera_frame_.c_str(), ex.what()); return -1; } // 0 is right, 1 is down-right, 2 is down, 3 is down-left, 4 is left, 5 is up-left, 6 is up, 7 is up-right RCLCPP_INFO( - node_->get_logger(), "[IsPointing] Low pointing limit %d. High point limit %d", low_pointing_limit_, + node_->get_logger(), "Low pointing limit %d. High point limit %d", low_pointing_limit_, high_pointing_limit_); for (int i = low_pointing_limit_; i <= high_pointing_limit_; i++) { RCLCPP_INFO( - node_->get_logger(), "[IsPointing] Pointing direction %d", + node_->get_logger(), "Pointing direction %d", detection.pointing_direction); if (detection.pointing_direction == i) { pointing_direction = i; @@ -147,18 +147,34 @@ BT::NodeStatus IsPointing::tick() best_detection = detections[0]; RCLCPP_INFO( - node_->get_logger(), "[IsPointing] Best detection: %s, color: %ld, pointing: %d", + node_->get_logger(), "Best detection: %s, color: %ld, pointing: %d", best_detection.unique_id.c_str(), best_detection.color_person, best_detection.pointing_direction); - int direction = publish_detection_tf(best_detection); + // int direction = publish_detection_tf(best_detection); + int direction; + pl::getInstance(node_)->publishTF(best_detection, output_frame_); + 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; + setOutput("pointing_direction", direction); + break; + } + } + if (direction == -1) { - RCLCPP_ERROR(node_->get_logger(), "[IsPointing] Error, invalid pointing direction"); + RCLCPP_ERROR(node_->get_logger(), "Error, invalid pointing direction"); return BT::NodeStatus::FAILURE; } - setOutput("output_frame", "someone_pointing"); - setOutput("pointing_direction", direction); - setOutput("person_id", best_detection.unique_id); + + // If someones is pointing, we return SUCCESS and populate output ports + setOutput("detection", best_detection); + setOutput("output_frame", output_frame_); + // setOutput("pointing_direction", direction); + // setOutput("person_id", best_detection.unique_id); return BT::NodeStatus::SUCCESS; } From 286db63acdba76782da1676bcec6d125d36858e2 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 22 Nov 2024 10:49:00 +0100 Subject: [PATCH 12/60] removed unnecessary dependency --- perception/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index 09ff908..bf91a7d 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_cascade_lifecycle REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) -find_package(yolov8_msgs REQUIRED) +# find_package(yolo_msgs REQUIRED) find_package(moveit_msgs REQUIRED) find_package(shape_msgs REQUIRED) find_package(rclpy REQUIRED) @@ -33,7 +33,7 @@ set(dependencies rclcpp_cascade_lifecycle rclpy behaviortree_cpp_v3 - yolov8_msgs + # yolo_msgs tf2_ros geometry_msgs tf2_geometry_msgs From d2f038df3ab64e994454c8cbd075fc18d235cd42 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 22 Nov 2024 10:49:09 +0100 Subject: [PATCH 13/60] dependency out --- perception/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/package.xml b/perception/package.xml index dc4705c..b48fa65 100644 --- a/perception/package.xml +++ b/perception/package.xml @@ -16,7 +16,7 @@ rclcpp rclcpp_cascade_lifecycle behaviortree_cpp_v3 - yolov8_msgs + yolo_msgs moveit_msgs shape_msgs rclpy From a4302b60af609f59e79835aa524c290db6830ff1 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 22 Nov 2024 10:49:16 +0100 Subject: [PATCH 14/60] refined xml --- motion/src/motion/navigation/NavigateTo.cpp | 7 +- perception/bt_xml/detect_chair.xml | 87 +++++++++++++++++++-- perception/bt_xml/in_front.xml | 7 +- perception/include/perception/IsInFront.hpp | 3 + perception/src/perception/IsDetected.cpp | 15 ++-- perception/src/perception/IsInFront.cpp | 5 ++ 6 files changed, 106 insertions(+), 18 deletions(-) diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index b15cf5c..d584469 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -48,7 +48,8 @@ void NavigateTo::on_tick() } catch (const tf2::TransformException & ex) { RCLCPP_WARN( node_->get_logger(), "Could not transform %s to %s: %s", "map", tf_frame.c_str(), ex.what()); - setStatus(BT::NodeStatus::RUNNING); + setStatus(BT::NodeStatus::FAILURE); + return; } // goal.pose.position.x = map_to_goal.transform.translation.x; @@ -64,7 +65,7 @@ void NavigateTo::on_tick() getInput("x", goal.pose.position.x); getInput("y", goal.pose.position.y); - RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %f, y: %f", goal.pose.position.x, goal.pose.position.y); + RCLCPP_INFO(node_->get_logger(), "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; @@ -75,7 +76,7 @@ void NavigateTo::on_tick() goal.header.frame_id = "map"; RCLCPP_INFO( - node_->get_logger(), "Sending coordinates. Frame: %s", goal.header.frame_id.c_str()); + node_->get_logger(), "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(), "Setting distance tolerance to %f", distance_tolerance); diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml index 64df3f2..fed351e 100644 --- a/perception/bt_xml/detect_chair.xml +++ b/perception/bt_xml/detect_chair.xml @@ -4,20 +4,36 @@ - - + + - + + + + + + + + + + Time needed to complete the gait speed test + + + + + + + + - @@ -27,6 +43,13 @@ + + + + + + + @@ -34,18 +57,68 @@ + + + + + + + + + + + + + + + How far from the target + True in case we do not want to reach the destination but to stay close Where to navigate False in case it is navigating to follow something - - + + + + + + + + Current interest of the robot. A static transform in relation to odom will be published + + + + - 1: left; -1: right + + + + + + + If false (default), the Subtree has an isolated blackboard and needs port remapping + + + If false , the Subtree has an isolated blackboard and needs port remapping + + + + If false , the Subtree has an isolated blackboard and needs port remapping + + + + + + If false (default), the Subtree has an isolated blackboard and needs port remapping + + + + + diff --git a/perception/bt_xml/in_front.xml b/perception/bt_xml/in_front.xml index b0582c9..99d2c25 100644 --- a/perception/bt_xml/in_front.xml +++ b/perception/bt_xml/in_front.xml @@ -4,11 +4,11 @@ - + - + @@ -17,11 +17,11 @@ + - @@ -35,6 +35,7 @@ + diff --git a/perception/include/perception/IsInFront.hpp b/perception/include/perception/IsInFront.hpp index 1ecca21..4532953 100644 --- a/perception/include/perception/IsInFront.hpp +++ b/perception/include/perception/IsInFront.hpp @@ -49,6 +49,7 @@ class IsInFront : public BT::ConditionNode return BT::PortsList( { BT::InputPort("confidence"), + BT::InputPort("entity_to_identify"), BT::InputPort("target"), BT::InputPort("what"), BT::OutputPort("direction") @@ -61,6 +62,8 @@ class IsInFront : public BT::ConditionNode std::string target_; double confidence_; + std::string entity_; + }; } // namespace perception diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/IsDetected.cpp index e80e58f..858d841 100644 --- a/perception/src/perception/IsDetected.cpp +++ b/perception/src/perception/IsDetected.cpp @@ -38,7 +38,8 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura node_->add_activation("perception_system/perception_object_detection"); } - + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + getInput("interest", interest_); getInput("cam_frame", cam_frame_); getInput("confidence", threshold_); @@ -68,7 +69,8 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - RCLCPP_WARN(node_->get_logger(), "No detections"); + // RCLCPP_WARN(node_->get_logger(), "No detections"); + RCLCPP_DEBUG(node_->get_logger(), "No detections"); return BT::NodeStatus::FAILURE; } @@ -108,10 +110,13 @@ BT::NodeStatus IsDetected::tick() it = detections.erase(it); } else { - frames_.push_back(detection.class_name + "_" + std::to_string(entity_counter + 1)); - if (pl::getInstance(node_)->publishTF(detection, std::to_string(entity_counter)) == -1) { + std::string tf_name; + tf_name = detection.class_name + "_" + std::to_string(entity_counter + 1); + frames_.push_back(tf_name); + if (pl::getInstance(node_)->publishTF(detection, tf_name) == -1) { return BT::NodeStatus::FAILURE; } + RCLCPP_INFO(node_->get_logger(), "Publishing TF %s", tf_name.c_str()); ++it; ++entity_counter; } @@ -128,7 +133,7 @@ BT::NodeStatus IsDetected::tick() setOutput("detection", detections[0]); setOutput("n_dectections", static_cast(detections.size()) + 1); // setOutput("frames", frames_); - setOutput("frame", frames_[0]); + setOutput("frame", frames_[0]); // TF frame of the best detected entity // frames_.clear(); diff --git a/perception/src/perception/IsInFront.cpp b/perception/src/perception/IsInFront.cpp index 83365eb..1f886c4 100644 --- a/perception/src/perception/IsInFront.cpp +++ b/perception/src/perception/IsInFront.cpp @@ -28,6 +28,7 @@ IsInFront::IsInFront(const std::string & xml_tag_name, const BT::NodeConfigurati getInput("target", target_); getInput("conficende", confidence_); getInput("what", what); + getInput("entity_to_identify", entity_); if (what == "person") { node_->add_activation("perception_system/perception_people_detection"); @@ -57,6 +58,8 @@ BT::NodeStatus IsInFront::tick() return BT::NodeStatus::FAILURE; } + detection = detections[0]; + // double dx = detection.center3d.position.x; // double dy = detection.center3d.position.y; // double yaw = atan2(dx, -dy); @@ -73,6 +76,8 @@ BT::NodeStatus IsInFront::tick() } // The detection is in front + // Publish the detection + pl::getInstance(node_)->publishTF(detection, entity_); setOutput("direction", 0); return BT::NodeStatus::SUCCESS; From e463163a9de272c9b4f2f7031f084b3f07bee54a Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 09:16:08 +0100 Subject: [PATCH 15/60] DialogConfirmation tested --- hri/bt_xml/confirm.xml | 29 +++++++++++++++++++++++++++++ hri/config/hri.yaml | 3 ++- 2 files changed, 31 insertions(+), 1 deletion(-) create mode 100644 hri/bt_xml/confirm.xml 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/config/hri.yaml b/hri/config/hri.yaml index f520208..79f3cd5 100644 --- a/hri/config/hri.yaml +++ b/hri/config/hri.yaml @@ -1,7 +1,7 @@ hri_node: ros__parameters: use_sim_time: False - bt_xml_file: query.xml + bt_xml_file: confirm.xml prompt_file: prompt.txt grammar_file: grammar.txt placeholder: "[]" @@ -9,4 +9,5 @@ hri_node: - speak_bt_node - listen_bt_node - query_bt_node + - dialog_confirmation_bt_node From ec94f4d088d1d731dc0fb302822115ee95ed781c Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 11:34:36 +0100 Subject: [PATCH 16/60] Ignore things --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 4ef01bc..8ce0960 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .gitingore _*.hpp -_*.cpp \ No newline at end of file +_*.cpp +_*.txt \ No newline at end of file From ac81dee995cdca8d5733c4e63fd050293873ee86 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 11:34:43 +0100 Subject: [PATCH 17/60] minor --- motion/src/motion/navigation/NavigateTo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index d584469..05fc41a 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -42,7 +42,7 @@ void NavigateTo::on_tick() getInput("distance_tolerance", distance_tolerance); 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()); + RCLCPP_INFO(node_->get_logger(), "Transforming map to %s", tf_frame.c_str()); try { map_to_goal = tf_buffer_.lookupTransform("map", tf_frame, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { From 162cf2f8897f514f9d403256674befce3a5be2cc Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 11:34:54 +0100 Subject: [PATCH 18/60] updated --- perception/CMakeLists.txt | 50 +++------ perception/bt_xml/detect_chair.xml | 95 +--------------- perception/bt_xml/in_front.xml | 28 ++--- perception/bt_xml/indentify.xml | 51 --------- perception/bt_xml/indentify_person.xml | 33 ++++++ perception/include/perception/Identify.hpp | 73 +++++++++++++ .../include/perception/IdentifyPerson.hpp | 9 +- perception/include/perception/IsDetected.hpp | 22 ++-- perception/include/perception/IsInFront.hpp | 13 +-- perception/include/perception/IsPointing.hpp | 6 +- perception/src/perception/Identify.cpp | 102 ++++++++++++++++++ perception/src/perception/IsDetected.cpp | 12 +-- perception/src/perception/IsInFront.cpp | 36 ++++--- perception/src/perception/IsPointing.cpp | 3 +- 14 files changed, 289 insertions(+), 244 deletions(-) delete mode 100644 perception/bt_xml/indentify.xml create mode 100644 perception/bt_xml/indentify_person.xml create mode 100644 perception/include/perception/Identify.hpp create mode 100644 perception/src/perception/Identify.cpp diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index bf91a7d..5ca4d3e 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_cascade_lifecycle REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) -# find_package(yolo_msgs REQUIRED) find_package(moveit_msgs REQUIRED) find_package(shape_msgs REQUIRED) find_package(rclpy REQUIRED) @@ -33,13 +32,11 @@ set(dependencies rclcpp_cascade_lifecycle rclpy behaviortree_cpp_v3 - # yolo_msgs tf2_ros geometry_msgs tf2_geometry_msgs tf2 rclcpp_action - behaviortree_cpp_v3 perception_system OpenCV perception_system_interfaces @@ -49,30 +46,28 @@ set(dependencies include_directories(include) -add_library(is_detected_bt_node SHARED src/perception/IsDetected.cpp) -add_library(filter_entity_bt_node SHARED src/perception/FilterEntity.cpp) -add_library(is_pointing_bt_node SHARED src/perception/IsPointing.cpp) -add_library(identify_person_bt_node SHARED src/perception/IdentifyPerson.cpp) -add_library(get_angle_bt_node SHARED src/perception/GetAngle.cpp) -add_library(is_in_front_bt_node SHARED src/perception/IsInFront.cpp) - -list(APPEND plugin_libs - is_detected_bt_node - filter_entity_bt_node - is_pointing_bt_node - identify_person_bt_node - get_angle_bt_node - is_in_front_bt_node +set(plugin_sources + src/perception/IsDetected.cpp + src/perception/FilterEntity.cpp + src/perception/IsPointing.cpp + src/perception/IdentifyPerson.cpp + src/perception/Identify.cpp + src/perception/GetAngle.cpp + src/perception/IsInFront.cpp ) -foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) - target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +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}) -target_link_libraries(perception_test ${ZMQ_LIBRARIES}) install(TARGETS perception_test @@ -87,22 +82,9 @@ install(DIRECTORY include launch config bt_xml DESTINATION share/${PROJECT_NAME} ) -install(PROGRAMS - DESTINATION lib/${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_dependencies(${dependencies}) -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) - -foreach(bt_plugin ${plugin_libs}) - ament_export_libraries(${bt_plugin}) -endforeach() - ament_package() diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml index fed351e..9b6198d 100644 --- a/perception/bt_xml/detect_chair.xml +++ b/perception/bt_xml/detect_chair.xml @@ -4,121 +4,36 @@ - + - + - - - - - - - - - Time needed to complete the gait speed test - - - - - - - - - - - - - + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - How far from the target True in case we do not want to reach the destination but to stay close Where to navigate False in case it is navigating to follow something - - - - - - - Current interest of the robot. A static transform in relation to odom will be published - - - - - - - - - - - If false (default), the Subtree has an isolated blackboard and needs port remapping - - - If false , the Subtree has an isolated blackboard and needs port remapping - - - - If false , the Subtree has an isolated blackboard and needs port remapping - - - - - - If false (default), the Subtree has an isolated blackboard and needs port remapping - - - - - diff --git a/perception/bt_xml/in_front.xml b/perception/bt_xml/in_front.xml index 99d2c25..a306aa7 100644 --- a/perception/bt_xml/in_front.xml +++ b/perception/bt_xml/in_front.xml @@ -4,31 +4,32 @@ - + + - + - + - - + - + + @@ -36,21 +37,8 @@ - + - - - - - - - - - How far from the target - True in case we do not want to reach the destination but to stay close - Where to navigate - False in case it is navigating to follow something - diff --git a/perception/bt_xml/indentify.xml b/perception/bt_xml/indentify.xml deleted file mode 100644 index fba076b..0000000 --- a/perception/bt_xml/indentify.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - How far from the target - True in case we do not want to reach the destination but to stay close - Where to navigate - False in case it is navigating to follow something - - - - 1: left; -1: right - - - - - diff --git a/perception/bt_xml/indentify_person.xml b/perception/bt_xml/indentify_person.xml new file mode 100644 index 0000000..d78296a --- /dev/null +++ b/perception/bt_xml/indentify_person.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/include/perception/Identify.hpp b/perception/include/perception/Identify.hpp new file mode 100644 index 0000000..ed3b08d --- /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: + std::shared_ptr node_; + + std::string entity_; + bool detection_at_input_; + float confidence_; + + perception_system_interfaces::msg::Detection detection_; + +}; + +} // namespace perception + +#endif // IDENTIFY_HPP_ \ No newline at end of file diff --git a/perception/include/perception/IdentifyPerson.hpp b/perception/include/perception/IdentifyPerson.hpp index a167dd0..dd88e13 100644 --- a/perception/include/perception/IdentifyPerson.hpp +++ b/perception/include/perception/IdentifyPerson.hpp @@ -49,10 +49,11 @@ class IdentifyPerson : public BT::ActionNodeBase { return BT::PortsList( { - BT::InputPort("person_to_identify"), + BT::InputPort("person_to_identify"), // Name of the person to identify. Will be used as key in the blackboard + // A TF frame with the name of the person will be published BT::InputPort("get_features"), // If true, features are saved. If false, features were already saved - BT::InputPort("confidence"), - BT::InputPort("detection"), + BT::InputPort("confidence"), // Confidence threshold + BT::InputPort("detection"), // Detection of the person to identify }); } @@ -69,4 +70,4 @@ class IdentifyPerson : public BT::ActionNodeBase } // namespace perception -#endif // IDENTIFY_PERSON_HPP_ +#endif // IDENTIFY_PERSON_HPP_ \ No newline at end of file diff --git a/perception/include/perception/IsDetected.hpp b/perception/include/perception/IsDetected.hpp index a77a2ef..c9eb78e 100644 --- a/perception/include/perception/IsDetected.hpp +++ b/perception/include/perception/IsDetected.hpp @@ -49,18 +49,16 @@ class IsDetected : public BT::ConditionNode { return BT::PortsList( { - BT::InputPort("max_entities"), - // BT::InputPort("person_id"), - BT::InputPort("what"), - BT::InputPort("cam_frame"), - BT::InputPort("interest"), - BT::InputPort("confidence"), - BT::InputPort("order"), - BT::InputPort("max_depth"), - BT::OutputPort("frame"), // TF frame of the detected entity - // BT::OutputPort("best_detection"), - BT::OutputPort("detection"), - BT::OutputPort("n_detections"), + BT::InputPort("max_entities"), // Max number of entities to consider + BT::InputPort("model"), // YOLO model (object or person) + BT::InputPort("cam_frame"), // Camera frame + 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::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) }); } diff --git a/perception/include/perception/IsInFront.hpp b/perception/include/perception/IsInFront.hpp index 4532953..fc59480 100644 --- a/perception/include/perception/IsInFront.hpp +++ b/perception/include/perception/IsInFront.hpp @@ -48,10 +48,11 @@ class IsInFront : public BT::ConditionNode { return BT::PortsList( { - BT::InputPort("confidence"), - BT::InputPort("entity_to_identify"), - BT::InputPort("target"), - BT::InputPort("what"), + BT::InputPort("confidence"), // Confidence threshold + BT::InputPort("entity_to_identify"), // Name of the entity to check if is in front. Will be used as key in the blackboard + // A TF frame with this name will be published + BT::InputPort("model"), // YOLO model to use (people, object) + BT::InputPort("detection"), // Detection to check if is in front (optional) BT::OutputPort("direction") }); } @@ -59,10 +60,10 @@ class IsInFront : public BT::ConditionNode private: std::shared_ptr node_; - std::string target_; double confidence_; - std::string entity_; + bool detection_at_input_; + perception_system_interfaces::msg::Detection detection_; }; diff --git a/perception/include/perception/IsPointing.hpp b/perception/include/perception/IsPointing.hpp index 22f1bba..bf7806a 100644 --- a/perception/include/perception/IsPointing.hpp +++ b/perception/include/perception/IsPointing.hpp @@ -50,14 +50,12 @@ class IsPointing : public BT::ConditionNode { return BT::PortsList( { - // BT::InputPort("person_id"), BT::InputPort("camera_frame"), BT::InputPort("low_pointing_limit"), BT::InputPort("high_pointing_limit"), - BT::InputPort("output_frame"), - BT::OutputPort("output_frame"), + // BT::InputPort("output_frame"), + BT::BidirectionalPort("output_frame"), BT::OutputPort("pointing_direction"), - // BT::OutputPort("person_id"), BT::OutputPort("detection") }); } diff --git a/perception/src/perception/Identify.cpp b/perception/src/perception/Identify.cpp new file mode 100644 index 0000000..466e372 --- /dev/null +++ b/perception/src/perception/Identify.cpp @@ -0,0 +1,102 @@ +// 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.5) +{ + config().blackboard->get("node", node_); + + getInput("entity_to_identify", entity_); + + detection_at_input_ = true; + if (!getInput("detection", detection_)) { + detection_at_input_ = false; + } + +} + +BT::NodeStatus +Identify::tick() +{ + std::vector detections; + // perception_system_interfaces::msg::Detection entity_detection; + try + { + + if (detection_at_input_) // Configure perception system to track the specified person + { + RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", entity_.c_str()); + // detections = pl::getInstance(node_)->get_by_type("person"); + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return a.center3d.position.z < b.center3d.position.z; + // }); + // getInput("detection", entity_detection); + // detections = pl::getInstance(node_)->set_features_of_interest(entity_detection); + config().blackboard->set(entity_, detections[0]); + } + + RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", entity_.c_str()); + + if (!detection_at_input_) { + config().blackboard->get(entity_, detection_); + } + + detections = pl::getInstance(node_)->get_by_features(detection_, confidence_); + + std::sort( + detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + return a.score > b.score; + }); + + if (detections.empty()) + { + RCLCPP_WARN(node_->get_logger(), "%s NOT detected", entity_.c_str()); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_DEBUG(node_->get_logger(), "%s detected with confidence %f", entity_.c_str(), detections[0].score); + + // Publish the detection + pl::getInstance(node_)->publishTF(detections[0], entity_); + + return BT::NodeStatus::SUCCESS; + } + catch(const std::exception& ex) + { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + return BT::NodeStatus::FAILURE; + } + + return BT::NodeStatus::RUNNING; +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("Identify"); +} + diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/IsDetected.cpp index 858d841..de03544 100644 --- a/perception/src/perception/IsDetected.cpp +++ b/perception/src/perception/IsDetected.cpp @@ -25,16 +25,16 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura max_depth_(std::numeric_limits::max()), max_entities_(1) { - std::string what; + std::string model; config().blackboard->get("node", node_); - config().blackboard->get("what", what); + config().blackboard->get("model", model); - if (what == "person") { + if (model == "person") { node_->add_activation("perception_system/perception_people_detection"); - } else if (what == "object") { + } else if (model == "object") { node_->add_activation("perception_system/perception_object_detection"); } else { - RCLCPP_ERROR(node_->get_logger(), "Unknown what: %s. Activating generic", what.c_str()); + RCLCPP_ERROR(node_->get_logger(), "Unknown model: %s. Activating generic", model.c_str()); node_->add_activation("perception_system/perception_object_detection"); } @@ -130,7 +130,7 @@ BT::NodeStatus IsDetected::tick() RCLCPP_INFO(node_->get_logger(), "A %s has been found with the provided conditions", detections[0].class_name.c_str()); // setOutput("best_detection", detections[0].class_name); - setOutput("detection", detections[0]); + setOutput("best_detection", detections[0]); setOutput("n_dectections", static_cast(detections.size()) + 1); // setOutput("frames", frames_); setOutput("frame", frames_[0]); // TF frame of the best detected entity diff --git a/perception/src/perception/IsInFront.cpp b/perception/src/perception/IsInFront.cpp index 1f886c4..956d5f2 100644 --- a/perception/src/perception/IsInFront.cpp +++ b/perception/src/perception/IsInFront.cpp @@ -22,20 +22,25 @@ using pl = perception_system::PerceptionListener; IsInFront::IsInFront(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(xml_tag_name, conf) { - std::string what; + std::string model; config().blackboard->get("node", node_); - getInput("target", target_); - getInput("conficende", confidence_); - getInput("what", what); + + getInput("confidence", confidence_); + getInput("model", model); getInput("entity_to_identify", entity_); - if (what == "person") { + detection_at_input_ = true; + if (!getInput("detection", detection_)) { + detection_at_input_ = false; + } + + if (model == "people") { node_->add_activation("perception_system/perception_people_detection"); - } else if (what == "object") { + } else if (model == "object") { node_->add_activation("perception_system/perception_object_detection"); } else { - RCLCPP_ERROR(node_->get_logger(), "Unknown what: %s. Activating generic", what.c_str()); + RCLCPP_ERROR(node_->get_logger(), "Unknown model: %s. Activating generic", model.c_str()); node_->add_activation("perception_system/perception_object_detection"); } } @@ -44,13 +49,14 @@ BT::NodeStatus IsInFront::tick() RCLCPP_DEBUG(node_->get_logger(), "IsInFront ticked"); std::vector detections; - perception_system_interfaces::msg::Detection detection; - config().blackboard->get(target_, detection); + if (!detection_at_input_) { + config().blackboard->get(entity_, detection_); + } rclcpp::spin_some(node_->get_node_base_interface()); - detections = pl::getInstance(node_)->get_by_features(detection, confidence_); + detections = pl::getInstance(node_)->get_by_features(detection_, confidence_); if (detections.empty()) { RCLCPP_ERROR(node_->get_logger(), "No detections found"); @@ -58,26 +64,26 @@ BT::NodeStatus IsInFront::tick() return BT::NodeStatus::FAILURE; } - detection = detections[0]; + detection_ = detections[0]; // 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); + double yaw = atan2(detection_.center3d.position.y, detection_.center3d.position.x); yaw = yaw * 180.0 / M_PI; if (std::abs((yaw)) > 5.0) { // If the angle is greater than 5 degrees, the detection is not in front if (yaw > 0) { - setOutput("direction", 1); + setOutput("direction", 1); // Left } else { - setOutput("direction", -1); + setOutput("direction", -1); // Right } return BT::NodeStatus::FAILURE; } // The detection is in front // Publish the detection - pl::getInstance(node_)->publishTF(detection, entity_); + pl::getInstance(node_)->publishTF(detection_, entity_); setOutput("direction", 0); return BT::NodeStatus::SUCCESS; diff --git a/perception/src/perception/IsPointing.cpp b/perception/src/perception/IsPointing.cpp index 0aca8e7..a92eb8a 100644 --- a/perception/src/perception/IsPointing.cpp +++ b/perception/src/perception/IsPointing.cpp @@ -173,8 +173,7 @@ BT::NodeStatus IsPointing::tick() // If someones is pointing, we return SUCCESS and populate output ports setOutput("detection", best_detection); setOutput("output_frame", output_frame_); - // setOutput("pointing_direction", direction); - // setOutput("person_id", best_detection.unique_id); + setOutput("pointing_direction", direction); return BT::NodeStatus::SUCCESS; } From eef850ce8892ac4df3e49ae2c928c1da1b22eea2 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 12:14:24 +0100 Subject: [PATCH 19/60] correction --- perception/config/perception.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml index 29d04af..061578a 100644 --- a/perception/config/perception.yaml +++ b/perception/config/perception.yaml @@ -4,6 +4,6 @@ perception_node: bt_xml_file: detect_chair.xml plugins: - is_detected_bt_node - - identify_person_bt_node + - identify_bt_node - spin_bt_node - navigate_to_bt_node From c7ff86d4d88fa79d99449f22e96b7af4d68f69ea Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 25 Nov 2024 12:14:32 +0100 Subject: [PATCH 20/60] minor --- perception/src/perception/Identify.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/perception/src/perception/Identify.cpp b/perception/src/perception/Identify.cpp index 466e372..98c0c38 100644 --- a/perception/src/perception/Identify.cpp +++ b/perception/src/perception/Identify.cpp @@ -46,8 +46,7 @@ Identify::tick() try { - if (detection_at_input_) // Configure perception system to track the specified person - { + if (detection_at_input_) { // Configure perception system to track the specified person RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", entity_.c_str()); // detections = pl::getInstance(node_)->get_by_type("person"); // std::sort( @@ -55,7 +54,7 @@ Identify::tick() // return a.center3d.position.z < b.center3d.position.z; // }); // getInput("detection", entity_detection); - // detections = pl::getInstance(node_)->set_features_of_interest(entity_detection); + // detections = pl::getInstance(node_)->set_features_of_interest(detection_); config().blackboard->set(entity_, detections[0]); } From 1a4c2f543d3d0a355c584c007c2fddc7d5c11ed8 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 26 Nov 2024 12:03:11 +0100 Subject: [PATCH 21/60] refined --- motion/include/motion/base/Spin.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/motion/include/motion/base/Spin.hpp b/motion/include/motion/base/Spin.hpp index 750bdae..8b08307 100644 --- a/motion/include/motion/base/Spin.hpp +++ b/motion/include/motion/base/Spin.hpp @@ -46,10 +46,9 @@ class Spin : public BT::ActionNodeBase { return BT::PortsList( { - BT::InputPort("angle"), // if -1, it will spin forever - BT::InputPort("speed"), - // BT::InputPort("direction"), // 1: left, -1: right - BT::InputPort("forever") + BT::InputPort("angle"), // if negative, rotate right, if positive, rotate left + BT::InputPort("speed"), // rad/s + BT::InputPort("forever") // if true, keep rotating }); } From 155919561816491f3a47cf6473d68f95363f8fd3 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 27 Nov 2024 12:18:28 +0100 Subject: [PATCH 22/60] fix --- motion/src/motion/navigation/NavigateTo.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/NavigateTo.cpp index 05fc41a..fba0d43 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/NavigateTo.cpp @@ -52,15 +52,15 @@ void NavigateTo::on_tick() return; } - // 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; + 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; - goal.pose.position.z = 0.0; } else { // No TF, use coordinates getInput("x", goal.pose.position.x); From 4dda2f678247ae9a8b177f0fd2f78deed15bf203 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 27 Nov 2024 12:18:51 +0100 Subject: [PATCH 23/60] Needed parameter added --- perception/config/perception.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml index 061578a..915a305 100644 --- a/perception/config/perception.yaml +++ b/perception/config/perception.yaml @@ -1,6 +1,7 @@ perception_node: ros__parameters: use_sim_time: True + tf_frame_camera: head_front_camera_optical_frame bt_xml_file: detect_chair.xml plugins: - is_detected_bt_node From d20fa12e95db9a19481cbe7cd6761df848e1049a Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 27 Nov 2024 17:08:13 +0100 Subject: [PATCH 24/60] package refined --- hri/CMakeLists.txt | 8 +- ...nfirmation.hpp => dialog_confirmation.hpp} | 0 .../hri/dialog/{Listen.hpp => listen.hpp} | 0 .../hri/dialog/{Query.hpp => query.hpp} | 0 .../hri/dialog/{Speak.hpp => speak.hpp} | 0 ...nfirmation.cpp => dialog_confirmation.cpp} | 2 +- hri/src/hri/dialog/{Listen.cpp => listen.cpp} | 2 +- hri/src/hri/dialog/{Query.cpp => query.cpp} | 2 +- hri/src/hri/dialog/{Speak.cpp => speak.cpp} | 2 +- motion/CMakeLists.txt | 10 +- .../motion/base/{Face.hpp => face.hpp} | 0 .../motion/base/{Spin.hpp => spin.hpp} | 0 .../include/motion/head/{Pan.hpp => pan.hpp} | 0 ...vigateThrough.hpp => navigate_through.hpp} | 0 .../{NavigateTo.hpp => navigate_to.hpp} | 0 motion/src/motion/base/{Face.cpp => face.cpp} | 2 +- motion/src/motion/base/{Spin.cpp => spin.cpp} | 2 +- motion/src/motion/head/{Pan.cpp => pan.cpp} | 2 +- ...vigateThrough.cpp => navigate_through.cpp} | 2 +- .../{NavigateTo.cpp => navigate_to.cpp} | 2 +- perception/CMakeLists.txt | 13 ++- perception/bt_xml/detect_chair.xml | 12 ++- ...dentify_person.xml => identify_person.xml} | 6 +- perception/config/perception.yaml | 4 +- .../include/perception/IdentifyPerson.hpp | 73 -------------- .../{FilterEntity.hpp => filter_entity.hpp} | 14 +-- .../{GetAngle.hpp => get_angle.hpp} | 0 .../perception/{Identify.hpp => identify.hpp} | 0 .../{IsDetected.hpp => is_detected.hpp} | 3 +- .../{IsInFront.hpp => is_in_front.hpp} | 0 .../{IsPointing.hpp => is_pointing.hpp} | 0 perception/src/main_perception.cpp | 1 + perception/src/perception/IdentifyPerson.cpp | 97 ------------------- .../{FilterEntity.cpp => filter_entity.cpp} | 36 ++++--- .../{GetAngle.cpp => get_angle.cpp} | 2 +- .../perception/{Identify.cpp => identify.cpp} | 25 +++-- .../{IsDetected.cpp => is_detected.cpp} | 28 +++--- .../{IsInFront.cpp => is_in_front.cpp} | 4 +- .../{IsPointing.cpp => is_pointing.cpp} | 4 +- 39 files changed, 107 insertions(+), 251 deletions(-) rename hri/include/hri/dialog/{DialogConfirmation.hpp => dialog_confirmation.hpp} (100%) rename hri/include/hri/dialog/{Listen.hpp => listen.hpp} (100%) rename hri/include/hri/dialog/{Query.hpp => query.hpp} (100%) rename hri/include/hri/dialog/{Speak.hpp => speak.hpp} (100%) rename hri/src/hri/dialog/{DialogConfirmation.cpp => dialog_confirmation.cpp} (98%) rename hri/src/hri/dialog/{Listen.cpp => listen.cpp} (98%) rename hri/src/hri/dialog/{Query.cpp => query.cpp} (99%) rename hri/src/hri/dialog/{Speak.cpp => speak.cpp} (99%) rename motion/include/motion/base/{Face.hpp => face.hpp} (100%) rename motion/include/motion/base/{Spin.hpp => spin.hpp} (100%) rename motion/include/motion/head/{Pan.hpp => pan.hpp} (100%) rename motion/include/motion/navigation/{NavigateThrough.hpp => navigate_through.hpp} (100%) rename motion/include/motion/navigation/{NavigateTo.hpp => navigate_to.hpp} (100%) rename motion/src/motion/base/{Face.cpp => face.cpp} (99%) rename motion/src/motion/base/{Spin.cpp => spin.cpp} (99%) rename motion/src/motion/head/{Pan.cpp => pan.cpp} (99%) rename motion/src/motion/navigation/{NavigateThrough.cpp => navigate_through.cpp} (99%) rename motion/src/motion/navigation/{NavigateTo.cpp => navigate_to.cpp} (98%) rename perception/bt_xml/{indentify_person.xml => identify_person.xml} (77%) delete mode 100644 perception/include/perception/IdentifyPerson.hpp rename perception/include/perception/{FilterEntity.hpp => filter_entity.hpp} (95%) rename perception/include/perception/{GetAngle.hpp => get_angle.hpp} (100%) rename perception/include/perception/{Identify.hpp => identify.hpp} (100%) rename perception/include/perception/{IsDetected.hpp => is_detected.hpp} (97%) rename perception/include/perception/{IsInFront.hpp => is_in_front.hpp} (100%) rename perception/include/perception/{IsPointing.hpp => is_pointing.hpp} (100%) delete mode 100644 perception/src/perception/IdentifyPerson.cpp rename perception/src/perception/{FilterEntity.cpp => filter_entity.cpp} (78%) rename perception/src/perception/{GetAngle.cpp => get_angle.cpp} (98%) rename perception/src/perception/{Identify.cpp => identify.cpp} (75%) rename perception/src/perception/{IsDetected.cpp => is_detected.cpp} (86%) rename perception/src/perception/{IsInFront.cpp => is_in_front.cpp} (96%) rename perception/src/perception/{IsPointing.cpp => is_pointing.cpp} (98%) diff --git a/hri/CMakeLists.txt b/hri/CMakeLists.txt index fa57994..e1b24db 100644 --- a/hri/CMakeLists.txt +++ b/hri/CMakeLists.txt @@ -47,10 +47,10 @@ set(dependencies 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/DialogConfirmation.cpp) -add_library(query_bt_node SHARED src/hri/dialog/Query.cpp) +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 diff --git a/hri/include/hri/dialog/DialogConfirmation.hpp b/hri/include/hri/dialog/dialog_confirmation.hpp similarity index 100% rename from hri/include/hri/dialog/DialogConfirmation.hpp rename to hri/include/hri/dialog/dialog_confirmation.hpp diff --git a/hri/include/hri/dialog/Listen.hpp b/hri/include/hri/dialog/listen.hpp similarity index 100% rename from hri/include/hri/dialog/Listen.hpp rename to hri/include/hri/dialog/listen.hpp diff --git a/hri/include/hri/dialog/Query.hpp b/hri/include/hri/dialog/query.hpp similarity index 100% rename from hri/include/hri/dialog/Query.hpp rename to hri/include/hri/dialog/query.hpp diff --git a/hri/include/hri/dialog/Speak.hpp b/hri/include/hri/dialog/speak.hpp similarity index 100% rename from hri/include/hri/dialog/Speak.hpp rename to hri/include/hri/dialog/speak.hpp diff --git a/hri/src/hri/dialog/DialogConfirmation.cpp b/hri/src/hri/dialog/dialog_confirmation.cpp similarity index 98% rename from hri/src/hri/dialog/DialogConfirmation.cpp rename to hri/src/hri/dialog/dialog_confirmation.cpp index a4169a0..30d3a9b 100644 --- a/hri/src/hri/dialog/DialogConfirmation.cpp +++ b/hri/src/hri/dialog/dialog_confirmation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "hri/dialog/DialogConfirmation.hpp" +#include "hri/dialog/dialog_confirmation.hpp" diff --git a/hri/src/hri/dialog/Listen.cpp b/hri/src/hri/dialog/listen.cpp similarity index 98% rename from hri/src/hri/dialog/Listen.cpp rename to hri/src/hri/dialog/listen.cpp index baf0741..83f0b69 100644 --- a/hri/src/hri/dialog/Listen.cpp +++ b/hri/src/hri/dialog/listen.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "hri/dialog/Listen.hpp" +#include "hri/dialog/listen.hpp" namespace dialog { diff --git a/hri/src/hri/dialog/Query.cpp b/hri/src/hri/dialog/query.cpp similarity index 99% rename from hri/src/hri/dialog/Query.cpp rename to hri/src/hri/dialog/query.cpp index 59bc370..117d996 100644 --- a/hri/src/hri/dialog/Query.cpp +++ b/hri/src/hri/dialog/query.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "hri/dialog/Query.hpp" +#include "hri/dialog/query.hpp" namespace dialog { diff --git a/hri/src/hri/dialog/Speak.cpp b/hri/src/hri/dialog/speak.cpp similarity index 99% rename from hri/src/hri/dialog/Speak.cpp rename to hri/src/hri/dialog/speak.cpp index 29100f5..e1104b8 100644 --- a/hri/src/hri/dialog/Speak.cpp +++ b/hri/src/hri/dialog/speak.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "hri/dialog/Speak.hpp" +#include "hri/dialog/speak.hpp" namespace dialog { diff --git a/motion/CMakeLists.txt b/motion/CMakeLists.txt index c8b0de0..5209362 100644 --- a/motion/CMakeLists.txt +++ b/motion/CMakeLists.txt @@ -39,11 +39,11 @@ set(dependencies include_directories(include) -add_library(navigate_to_bt_node SHARED src/motion/navigation/NavigateTo.cpp) -add_library(navigate_through_bt_node SHARED src/motion/navigation/NavigateThrough.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) +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 diff --git a/motion/include/motion/base/Face.hpp b/motion/include/motion/base/face.hpp similarity index 100% rename from motion/include/motion/base/Face.hpp rename to motion/include/motion/base/face.hpp diff --git a/motion/include/motion/base/Spin.hpp b/motion/include/motion/base/spin.hpp similarity index 100% rename from motion/include/motion/base/Spin.hpp rename to motion/include/motion/base/spin.hpp diff --git a/motion/include/motion/head/Pan.hpp b/motion/include/motion/head/pan.hpp similarity index 100% rename from motion/include/motion/head/Pan.hpp rename to motion/include/motion/head/pan.hpp diff --git a/motion/include/motion/navigation/NavigateThrough.hpp b/motion/include/motion/navigation/navigate_through.hpp similarity index 100% rename from motion/include/motion/navigation/NavigateThrough.hpp rename to motion/include/motion/navigation/navigate_through.hpp diff --git a/motion/include/motion/navigation/NavigateTo.hpp b/motion/include/motion/navigation/navigate_to.hpp similarity index 100% rename from motion/include/motion/navigation/NavigateTo.hpp rename to motion/include/motion/navigation/navigate_to.hpp diff --git a/motion/src/motion/base/Face.cpp b/motion/src/motion/base/face.cpp similarity index 99% rename from motion/src/motion/base/Face.cpp rename to motion/src/motion/base/face.cpp index 56b63d9..b175d48 100644 --- a/motion/src/motion/base/Face.cpp +++ b/motion/src/motion/base/face.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/base/Face.hpp" +#include "motion/base/face.hpp" namespace base { diff --git a/motion/src/motion/base/Spin.cpp b/motion/src/motion/base/spin.cpp similarity index 99% rename from motion/src/motion/base/Spin.cpp rename to motion/src/motion/base/spin.cpp index 2904f69..21ac6ac 100644 --- a/motion/src/motion/base/Spin.cpp +++ b/motion/src/motion/base/spin.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/base/Spin.hpp" +#include "motion/base/spin.hpp" namespace base { diff --git a/motion/src/motion/head/Pan.cpp b/motion/src/motion/head/pan.cpp similarity index 99% rename from motion/src/motion/head/Pan.cpp rename to motion/src/motion/head/pan.cpp index c342b3c..b9db0b9 100644 --- a/motion/src/motion/head/Pan.cpp +++ b/motion/src/motion/head/pan.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/head/Pan.hpp" +#include "motion/head/pan.hpp" namespace head { diff --git a/motion/src/motion/navigation/NavigateThrough.cpp b/motion/src/motion/navigation/navigate_through.cpp similarity index 99% rename from motion/src/motion/navigation/NavigateThrough.cpp rename to motion/src/motion/navigation/navigate_through.cpp index 1a8ca44..972b429 100644 --- a/motion/src/motion/navigation/NavigateThrough.cpp +++ b/motion/src/motion/navigation/navigate_through.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/navigation/NavigateThrough.hpp" +#include "motion/navigation/navigate_through.hpp" namespace navigation diff --git a/motion/src/motion/navigation/NavigateTo.cpp b/motion/src/motion/navigation/navigate_to.cpp similarity index 98% rename from motion/src/motion/navigation/NavigateTo.cpp rename to motion/src/motion/navigation/navigate_to.cpp index fba0d43..14531a3 100644 --- a/motion/src/motion/navigation/NavigateTo.cpp +++ b/motion/src/motion/navigation/navigate_to.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/navigation/NavigateTo.hpp" +#include "motion/navigation/navigate_to.hpp" namespace navigation { diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index 5ca4d3e..3759f90 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -47,13 +47,12 @@ set(dependencies include_directories(include) set(plugin_sources - src/perception/IsDetected.cpp - src/perception/FilterEntity.cpp - src/perception/IsPointing.cpp - src/perception/IdentifyPerson.cpp - src/perception/Identify.cpp - src/perception/GetAngle.cpp - src/perception/IsInFront.cpp + 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 ) set(plugin_libs "") diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml index 9b6198d..56448d6 100644 --- a/perception/bt_xml/detect_chair.xml +++ b/perception/bt_xml/detect_chair.xml @@ -7,27 +7,33 @@ - + + + + + - + + How far from the target - True in case we do not want to reach the destination but to stay close Where to navigate False in case it is navigating to follow something + + diff --git a/perception/bt_xml/indentify_person.xml b/perception/bt_xml/identify_person.xml similarity index 77% rename from perception/bt_xml/indentify_person.xml rename to perception/bt_xml/identify_person.xml index d78296a..550090c 100644 --- a/perception/bt_xml/indentify_person.xml +++ b/perception/bt_xml/identify_person.xml @@ -3,7 +3,11 @@ - + + + + + diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml index 915a305..0fd83d4 100644 --- a/perception/config/perception.yaml +++ b/perception/config/perception.yaml @@ -1,8 +1,8 @@ perception_node: ros__parameters: use_sim_time: True - tf_frame_camera: head_front_camera_optical_frame - bt_xml_file: detect_chair.xml + # tf_frame_camera: head_front_camera_optical_frame + bt_xml_file: identify_person.xml plugins: - is_detected_bt_node - identify_bt_node diff --git a/perception/include/perception/IdentifyPerson.hpp b/perception/include/perception/IdentifyPerson.hpp deleted file mode 100644 index dd88e13..0000000 --- a/perception/include/perception/IdentifyPerson.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// 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_PERSON_HPP_ -#define IDENTIFY_PERSON_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 IdentifyPerson : public BT::ActionNodeBase -{ -public: - IdentifyPerson(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("person_to_identify"), // Name of the person to identify. Will be used as key in the blackboard - // A TF frame with the name of the person will be published - BT::InputPort("get_features"), // If true, features are saved. If false, features were already saved - BT::InputPort("confidence"), // Confidence threshold - BT::InputPort("detection"), // Detection of the person to identify - }); - } - -private: - std::shared_ptr node_; - std::shared_ptr static_broadcaster_; - - std::string person_; - bool get_features_; - - float confidence_; - -}; - -} // namespace perception - -#endif // IDENTIFY_PERSON_HPP_ \ No newline at end of file diff --git a/perception/include/perception/FilterEntity.hpp b/perception/include/perception/filter_entity.hpp similarity index 95% rename from perception/include/perception/FilterEntity.hpp rename to perception/include/perception/filter_entity.hpp index ab002a2..134ecd2 100644 --- a/perception/include/perception/FilterEntity.hpp +++ b/perception/include/perception/filter_entity.hpp @@ -52,12 +52,16 @@ class FilterEntity : public BT::ActionNodeBase return BT::PortsList( { BT::InputPort("frame"), - BT::InputPort("lambda", "filtering parameter"), - BT::OutputPort("filtered_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_; @@ -69,11 +73,7 @@ class FilterEntity : public BT::ActionNodeBase std::unique_ptr tf_broadcaster_; geometry_msgs::msg::TransformStamped filtered_entity_; - - geometry_msgs::msg::TransformStamped initialize_state_observer( - const geometry_msgs::msg::TransformStamped & entity); - geometry_msgs::msg::TransformStamped update_state_observer( - const geometry_msgs::msg::TransformStamped & entity); + }; } // namespace perception diff --git a/perception/include/perception/GetAngle.hpp b/perception/include/perception/get_angle.hpp similarity index 100% rename from perception/include/perception/GetAngle.hpp rename to perception/include/perception/get_angle.hpp diff --git a/perception/include/perception/Identify.hpp b/perception/include/perception/identify.hpp similarity index 100% rename from perception/include/perception/Identify.hpp rename to perception/include/perception/identify.hpp diff --git a/perception/include/perception/IsDetected.hpp b/perception/include/perception/is_detected.hpp similarity index 97% rename from perception/include/perception/IsDetected.hpp rename to perception/include/perception/is_detected.hpp index c9eb78e..0c5ddaa 100644 --- a/perception/include/perception/IsDetected.hpp +++ b/perception/include/perception/is_detected.hpp @@ -30,6 +30,7 @@ #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" @@ -50,7 +51,7 @@ class IsDetected : public BT::ConditionNode return BT::PortsList( { BT::InputPort("max_entities"), // Max number of entities to consider - BT::InputPort("model"), // YOLO model (object or person) + BT::InputPort("model"), // YOLO model (object or people) BT::InputPort("cam_frame"), // Camera frame BT::InputPort("interest"), // What to look for BT::InputPort("confidence"), // Confidence threshold diff --git a/perception/include/perception/IsInFront.hpp b/perception/include/perception/is_in_front.hpp similarity index 100% rename from perception/include/perception/IsInFront.hpp rename to perception/include/perception/is_in_front.hpp diff --git a/perception/include/perception/IsPointing.hpp b/perception/include/perception/is_pointing.hpp similarity index 100% rename from perception/include/perception/IsPointing.hpp rename to perception/include/perception/is_pointing.hpp diff --git a/perception/src/main_perception.cpp b/perception/src/main_perception.cpp index f698f1d..01d27e2 100644 --- a/perception/src/main_perception.cpp +++ b/perception/src/main_perception.cpp @@ -35,6 +35,7 @@ int main(int argc, char * argv[]) 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); diff --git a/perception/src/perception/IdentifyPerson.cpp b/perception/src/perception/IdentifyPerson.cpp deleted file mode 100644 index 77175cb..0000000 --- a/perception/src/perception/IdentifyPerson.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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/IdentifyPerson.hpp" - -namespace perception -{ - -using pl = perception_system::PerceptionListener; - -IdentifyPerson::IdentifyPerson( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) -: BT::ActionNodeBase(xml_tag_name, conf), - person_(""), - get_features_(false), - confidence_(0.5) -{ - config().blackboard->get("node", node_); - - getInput("person_to_identify", person_); - getInput("get_features", get_features_); - - static_broadcaster_ = std::make_shared(node_); -} - -BT::NodeStatus -IdentifyPerson::tick() -{ - std::vector detections; - perception_system_interfaces::msg::Detection person_detection; - try - { - - if (get_features_) // Configure perception system to track the specified person - { - RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", person_.c_str()); - // detections = pl::getInstance(node_)->get_by_type("person"); - // std::sort( - // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - // return a.center3d.position.z < b.center3d.position.z; - // }); - getInput("detection", person_detection); - // detections = pl::getInstance(node_)->set_features_of_interest(person_detection); - config().blackboard->set(person_, detections[0]); - } - - RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", person_.c_str()); - - config().blackboard->get(person_, person_detection); - detections = pl::getInstance(node_)->get_by_features(person_detection, confidence_); - - std::sort( - detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - return a.score > b.score; - }); - - if (detections.empty()) - { - RCLCPP_WARN(node_->get_logger(), "%s NOT detected", person_.c_str()); - return BT::NodeStatus::FAILURE; - } - - RCLCPP_DEBUG(node_->get_logger(), "%s detected with confidence %f", person_.c_str(), detections[0].score); - - // Publish the detection - pl::getInstance(node_)->publishTF(detections[0], person_); - - return BT::NodeStatus::SUCCESS; - } - catch(const std::exception& ex) - { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - return BT::NodeStatus::FAILURE; - } - - return BT::NodeStatus::RUNNING; -} - -} // namespace perception - -BT_REGISTER_NODES(factory) { - factory.registerNodeType("IdentifyPerson"); -} - diff --git a/perception/src/perception/FilterEntity.cpp b/perception/src/perception/filter_entity.cpp similarity index 78% rename from perception/src/perception/FilterEntity.cpp rename to perception/src/perception/filter_entity.cpp index dc33bfb..671127b 100644 --- a/perception/src/perception/FilterEntity.cpp +++ b/perception/src/perception/filter_entity.cpp @@ -12,13 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/FilterEntity.hpp" +#include "perception/filter_entity.hpp" -#include -#include -#include -#include "behaviortree_cpp_v3/behavior_tree.h" namespace perception { @@ -36,7 +32,6 @@ FilterEntity::FilterEntity(const std::string & xml_tag_name, const BT::NodeConfi getInput("lambda", lambda_); tf_broadcaster_ = std::make_unique(node_); - setOutput("filtered_frame", frame_ + "_filtered"); RCLCPP_INFO(node_->get_logger(), "FilterEntity initialized"); } @@ -68,34 +63,37 @@ geometry_msgs::msg::TransformStamped FilterEntity::initialize_state_observer( BT::NodeStatus FilterEntity::tick() { getInput("frame", frame_); - RCLCPP_INFO(node_->get_logger(), "IsMoving filtering frame %s", frame_.c_str()); + 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("odom", frame_, tf2::TimePointZero); + entity_transform_now_msg = tf_buffer_->lookupTransform("map", frame_, tf2::TimePointZero); RCLCPP_INFO( - node_->get_logger(), "Position %s to %s: %f %f %f", frame_.c_str(), "odom", + 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(), "odom", ex.what()); + 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; + 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.child_frame_id = frame_ + "_filtered"; + // 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); + // setOutput("filtered_frame", filtered_entity.child_frame_id); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/GetAngle.cpp b/perception/src/perception/get_angle.cpp similarity index 98% rename from perception/src/perception/GetAngle.cpp rename to perception/src/perception/get_angle.cpp index 33bf285..805774a 100644 --- a/perception/src/perception/GetAngle.cpp +++ b/perception/src/perception/get_angle.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/GetAngle.hpp" +#include "perception/get_angle.hpp" namespace perception { diff --git a/perception/src/perception/Identify.cpp b/perception/src/perception/identify.cpp similarity index 75% rename from perception/src/perception/Identify.cpp rename to perception/src/perception/identify.cpp index 98c0c38..60d79be 100644 --- a/perception/src/perception/Identify.cpp +++ b/perception/src/perception/identify.cpp @@ -13,7 +13,7 @@ // limitations under the License. -#include "perception/Identify.hpp" +#include "perception/identify.hpp" namespace perception { @@ -30,10 +30,10 @@ Identify::Identify( config().blackboard->get("node", node_); getInput("entity_to_identify", entity_); + RCLCPP_INFO(node_->get_logger(), "Identifying %s", entity_.c_str()); - detection_at_input_ = true; - if (!getInput("detection", detection_)) { - detection_at_input_ = false; + if (!getInput("confidence", confidence_)) { + RCLCPP_WARN(node_->get_logger(), "No confidence provided. Using default value %f", confidence_); } } @@ -43,6 +43,15 @@ Identify::tick() { std::vector detections; // perception_system_interfaces::msg::Detection entity_detection; + + // if (status() == BT::NodeStatus::IDLE) { + detection_at_input_ = true; + if (!getInput("detection", detection_)) { + RCLCPP_DEBUG(node_->get_logger(), "No detection at input"); + detection_at_input_ = false; + } + // } + try { @@ -55,15 +64,17 @@ Identify::tick() // }); // getInput("detection", entity_detection); // detections = pl::getInstance(node_)->set_features_of_interest(detection_); - config().blackboard->set(entity_, detections[0]); + config().blackboard->set(entity_, detection_); } RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", entity_.c_str()); if (!detection_at_input_) { config().blackboard->get(entity_, detection_); + RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); } + pl::getInstance(node_)->update(30); detections = pl::getInstance(node_)->get_by_features(detection_, confidence_); std::sort( @@ -77,10 +88,10 @@ Identify::tick() return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG(node_->get_logger(), "%s detected with confidence %f", entity_.c_str(), detections[0].score); + RCLCPP_INFO(node_->get_logger(), "%s detected with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); // Publish the detection - pl::getInstance(node_)->publishTF(detections[0], entity_); + pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/IsDetected.cpp b/perception/src/perception/is_detected.cpp similarity index 86% rename from perception/src/perception/IsDetected.cpp rename to perception/src/perception/is_detected.cpp index de03544..cee552c 100644 --- a/perception/src/perception/IsDetected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/IsDetected.hpp" +#include "perception/is_detected.hpp" namespace perception { @@ -25,28 +25,28 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura max_depth_(std::numeric_limits::max()), max_entities_(1) { - std::string model; config().blackboard->get("node", node_); - config().blackboard->get("model", model); - if (model == "person") { + std::string model; + getInput("model", model); + + if (model == "people") { + RCLCPP_INFO(node_->get_logger(), "Activating perception_people_detection"); node_->add_activation("perception_system/perception_people_detection"); } else if (model == "object") { + RCLCPP_INFO(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"); } - node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); - getInput("interest", interest_); getInput("cam_frame", cam_frame_); getInput("confidence", threshold_); getInput("max_entities", max_entities_); getInput("order", order_); getInput("max_depth", max_depth_); - // getInput("person_id", person_id_); RCLCPP_INFO(node_->get_logger(), "Interest: %s", interest_.c_str()); @@ -56,6 +56,12 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura BT::NodeStatus IsDetected::tick() { rclcpp::spin_some(node_->get_node_base_interface()); + // getInput("interest", interest_); + // getInput("cam_frame", cam_frame_); + // getInput("confidence", threshold_); + // getInput("max_entities", max_entities_); + // getInput("order", order_); + // getInput("max_depth", max_depth_); if (status() == BT::NodeStatus::IDLE) { RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); @@ -69,8 +75,8 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - // RCLCPP_WARN(node_->get_logger(), "No detections"); - RCLCPP_DEBUG(node_->get_logger(), "No detections"); + RCLCPP_WARN(node_->get_logger(), "No detections"); + // RCLCPP_DEBUG(node_->get_logger(), "No detections"); return BT::NodeStatus::FAILURE; } @@ -113,7 +119,7 @@ BT::NodeStatus IsDetected::tick() std::string tf_name; tf_name = detection.class_name + "_" + std::to_string(entity_counter + 1); frames_.push_back(tf_name); - if (pl::getInstance(node_)->publishTF(detection, tf_name) == -1) { + 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()); @@ -137,7 +143,7 @@ BT::NodeStatus IsDetected::tick() // frames_.clear(); - RCLCPP_DEBUG(node_->get_logger(), "Detections published"); + RCLCPP_INFO(node_->get_logger(), "Detections published"); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/IsInFront.cpp b/perception/src/perception/is_in_front.cpp similarity index 96% rename from perception/src/perception/IsInFront.cpp rename to perception/src/perception/is_in_front.cpp index 956d5f2..086c3fb 100644 --- a/perception/src/perception/IsInFront.cpp +++ b/perception/src/perception/is_in_front.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/IsInFront.hpp" +#include "perception/is_in_front.hpp" namespace perception { @@ -83,7 +83,7 @@ BT::NodeStatus IsInFront::tick() // The detection is in front // Publish the detection - pl::getInstance(node_)->publishTF(detection_, entity_); + pl::getInstance(node_)->publishTF_EKF(detection_, entity_, true); setOutput("direction", 0); return BT::NodeStatus::SUCCESS; diff --git a/perception/src/perception/IsPointing.cpp b/perception/src/perception/is_pointing.cpp similarity index 98% rename from perception/src/perception/IsPointing.cpp rename to perception/src/perception/is_pointing.cpp index a92eb8a..08a5e22 100644 --- a/perception/src/perception/IsPointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/IsPointing.hpp" +#include "perception/is_pointing.hpp" namespace perception @@ -153,7 +153,7 @@ BT::NodeStatus IsPointing::tick() // int direction = publish_detection_tf(best_detection); int direction; - pl::getInstance(node_)->publishTF(best_detection, output_frame_); + pl::getInstance(node_)->publishTF_EKF(best_detection, output_frame_, true); for (int i = low_pointing_limit_; i <= high_pointing_limit_; i++) { RCLCPP_INFO( node_->get_logger(), "Pointing direction %d", From 35a479f7df23fc6cf38a55da53de012faf3fc9cf Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 28 Nov 2024 14:23:16 +0100 Subject: [PATCH 25/60] IsPointing updated due to problems in real robot --- perception/include/perception/is_pointing.hpp | 2 + perception/src/perception/is_pointing.cpp | 93 ++----------------- 2 files changed, 9 insertions(+), 86 deletions(-) diff --git a/perception/include/perception/is_pointing.hpp b/perception/include/perception/is_pointing.hpp index bf7806a..3b57a98 100644 --- a/perception/include/perception/is_pointing.hpp +++ b/perception/include/perception/is_pointing.hpp @@ -31,6 +31,8 @@ #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" diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp index 08a5e22..218911d 100644 --- a/perception/src/perception/is_pointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -37,87 +37,6 @@ IsPointing::IsPointing(const std::string & xml_tag_name, const BT::NodeConfigura tf_static_broadcaster_ = std::make_shared(node_); } -// // Distance between two transformations -// double tfs_distance( -// const geometry_msgs::msg::TransformStamped & tf1, -// const geometry_msgs::msg::TransformStamped & tf2) -// { -// return sqrt( -// pow(tf1.transform.translation.x - tf2.transform.translation.x, 2) + -// pow(tf1.transform.translation.y - tf2.transform.translation.y, 2) + -// pow(tf1.transform.translation.z - tf2.transform.translation.z, 2)); -// } - -// // Mean of two transformations -// geometry_msgs::msg::TransformStamped tfs_mean( -// const geometry_msgs::msg::TransformStamped & tf1, -// const geometry_msgs::msg::TransformStamped & tf2) -// { -// geometry_msgs::msg::TransformStamped mean = tf1; - -// mean.transform.translation.x = (tf1.transform.translation.x + tf2.transform.translation.x) / 2; -// mean.transform.translation.y = (tf1.transform.translation.y + tf2.transform.translation.y) / 2; -// mean.transform.translation.z = (tf1.transform.translation.z + tf2.transform.translation.z) / 2; - -// return mean; -// } - -int IsPointing::publish_detection_tf( - const perception_system_interfaces::msg::Detection & detection) -{ - int pointing_direction = -1; - geometry_msgs::msg::TransformStamped map2camera_msg; - - RCLCPP_INFO( - node_->get_logger(), "Detectection in frame_id %s", - detection.header.frame_id.c_str()); - - try { - map2camera_msg = tf_buffer_->lookupTransform("map", camera_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - node_->get_logger(), "Could not transform %s to %s: %s", "map", - camera_frame_.c_str(), ex.what()); - return -1; - } - - // 0 is right, 1 is down-right, 2 is down, 3 is down-left, 4 is left, 5 is up-left, 6 is up, 7 is up-right - RCLCPP_INFO( - node_->get_logger(), "Low pointing limit %d. High point limit %d", low_pointing_limit_, - high_pointing_limit_); - for (int i = low_pointing_limit_; i <= high_pointing_limit_; i++) { - RCLCPP_INFO( - node_->get_logger(), "Pointing direction %d", - detection.pointing_direction); - if (detection.pointing_direction == i) { - pointing_direction = i; - - tf2::Transform camera2detection; - camera2detection.setOrigin( - tf2::Vector3( - detection.center3d.position.x, detection.center3d.position.y, - detection.center3d.position.z)); - camera2detection.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - - tf2::Transform map2camera; - tf2::fromMsg(map2camera_msg.transform, map2camera); - - tf2::Transform map2detection = map2camera * camera2detection; - - geometry_msgs::msg::TransformStamped map2detection_msg; - map2detection_msg.header.frame_id = "map"; - map2detection_msg.child_frame_id = output_frame_; - - map2detection_msg.transform = tf2::toMsg(map2detection); - map2detection_msg.transform.translation.z = 0.0; - - tf_static_broadcaster_->sendTransform(map2detection_msg); - break; - } - } - return pointing_direction; -} - BT::NodeStatus IsPointing::tick() { if (status() == BT::NodeStatus::IDLE) { @@ -151,22 +70,24 @@ BT::NodeStatus IsPointing::tick() best_detection.unique_id.c_str(), best_detection.color_person, best_detection.pointing_direction); - // int direction = publish_detection_tf(best_detection); - int direction; - pl::getInstance(node_)->publishTF_EKF(best_detection, output_frame_, true); + 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); + if (pl::getInstance(node_)->publishTF_EKF(best_detection, output_frame_, true) == -1) { + return BT::NodeStatus::FAILURE; + } break; } } - if (direction == -1) { - RCLCPP_ERROR(node_->get_logger(), "Error, invalid pointing direction"); + if (direction == -1) { // No match + RCLCPP_ERROR(node_->get_logger(), "Invalid pointing direction"); return BT::NodeStatus::FAILURE; } From 113a426dfcfc7d9e0e04a94de20b050f4f747313 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 28 Nov 2024 15:23:41 +0100 Subject: [PATCH 26/60] minor --- perception/src/perception/is_detected.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index cee552c..f3677f6 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -75,7 +75,7 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - RCLCPP_WARN(node_->get_logger(), "No detections"); + RCLCPP_ERROR(node_->get_logger(), "No detections"); // RCLCPP_DEBUG(node_->get_logger(), "No detections"); return BT::NodeStatus::FAILURE; } From 8009cf13ce93f9cbe26ef41642e6426f136cbfa6 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 28 Nov 2024 15:23:56 +0100 Subject: [PATCH 27/60] refined --- perception/include/perception/is_pointing.hpp | 10 +---- perception/src/perception/is_pointing.cpp | 42 ++++++++++++------- 2 files changed, 30 insertions(+), 22 deletions(-) diff --git a/perception/include/perception/is_pointing.hpp b/perception/include/perception/is_pointing.hpp index 3b57a98..048758d 100644 --- a/perception/include/perception/is_pointing.hpp +++ b/perception/include/perception/is_pointing.hpp @@ -70,15 +70,9 @@ class IsPointing : public BT::ConditionNode std::string camera_frame_, output_frame_; int low_pointing_limit_, high_pointing_limit_; int pointing_direction_; + float threshold_; - // std::int64_t person_id_; - - geometry_msgs::msg::TransformStamped person_pose_; - rclcpp::Time last_pose_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_static_broadcaster_; -}; + }; } // namespace perception diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp index 218911d..d3b03c6 100644 --- a/perception/src/perception/is_pointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -25,7 +25,7 @@ using pl = perception_system::PerceptionListener; IsPointing::IsPointing(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(xml_tag_name, conf), - tf_buffer_() +threshold_(0.6) { config().blackboard->get("node", node_); @@ -34,40 +34,54 @@ IsPointing::IsPointing(const std::string & xml_tag_name, const BT::NodeConfigura getInput("high_pointing_limit", high_pointing_limit_); getInput("output_frame", output_frame_); - tf_static_broadcaster_ = std::make_shared(node_); + RCLCPP_INFO(node_->get_logger(), "Activating perception_people_detection"); + node_->add_activation("perception_system/perception_people_detection"); } BT::NodeStatus IsPointing::tick() { + rclcpp::spin_some(node_->get_node_base_interface()); if (status() == BT::NodeStatus::IDLE) { - RCLCPP_INFO(node_->get_logger(), "IsPointing ticked while IDLE"); + RCLCPP_DEBUG(node_->get_logger(), "IsPointing ticked while IDLE"); } pl::getInstance(node_)->set_interest("person", true); - pl::getInstance(node_)->update(true); - rclcpp::spin_some(node_->get_node_base_interface()); + pl::getInstance(node_)->update(35); - std::vector detections; - detections = pl::getInstance(node_)->get_by_type("person"); + auto detections = pl::getInstance(node_)->get_by_type("person"); if (detections.empty()) { RCLCPP_ERROR(node_->get_logger(), "No detections"); return BT::NodeStatus::FAILURE; } - perception_system_interfaces::msg::Detection best_detection; - // 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; + } - best_detection = detections[0]; + auto best_detection = detections[0]; - RCLCPP_INFO( - node_->get_logger(), "Best detection: %s, color: %ld, pointing: %d", - best_detection.unique_id.c_str(), best_detection.color_person, + RCLCPP_INFO(node_->get_logger(), "Best detection pointing direction: %d", best_detection.pointing_direction); int direction = -1; @@ -91,7 +105,7 @@ BT::NodeStatus IsPointing::tick() return BT::NodeStatus::FAILURE; } - // If someones is pointing, we return SUCCESS and populate output ports + // If someones is pointing to a valid direction, return SUCCESS and populate output ports setOutput("detection", best_detection); setOutput("output_frame", output_frame_); setOutput("pointing_direction", direction); From d38278597e12b91fb53d5bfa76946a1c3366bc46 Mon Sep 17 00:00:00 2001 From: roi Date: Sat, 30 Nov 2024 18:44:30 +0100 Subject: [PATCH 28/60] IsPointing working in simulation --- perception/bt_xml/is_pointing.xml | 49 +++++++++++++++++++ perception/include/perception/is_pointing.hpp | 18 ++----- perception/src/perception/is_pointing.cpp | 17 ++++--- 3 files changed, 63 insertions(+), 21 deletions(-) create mode 100644 perception/bt_xml/is_pointing.xml 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/include/perception/is_pointing.hpp b/perception/include/perception/is_pointing.hpp index 048758d..78aa7ff 100644 --- a/perception/include/perception/is_pointing.hpp +++ b/perception/include/perception/is_pointing.hpp @@ -19,12 +19,7 @@ #include #include #include -#include -#include -#include -#include -#include #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" @@ -52,25 +47,22 @@ class IsPointing : public BT::ConditionNode { return BT::PortsList( { - BT::InputPort("camera_frame"), BT::InputPort("low_pointing_limit"), BT::InputPort("high_pointing_limit"), - // BT::InputPort("output_frame"), - BT::BidirectionalPort("output_frame"), + BT::InputPort("threshold"), + BT::OutputPort("detection"), BT::OutputPort("pointing_direction"), - BT::OutputPort("detection") + BT::OutputPort("output_frame") }); } private: - int publish_detection_tf(const perception_system_interfaces::msg::Detection & detection); - std::shared_ptr node_; - std::string camera_frame_, output_frame_; + std::string output_frame_ = "someone_pointing"; int low_pointing_limit_, high_pointing_limit_; int pointing_direction_; - float threshold_; + float threshold_ = 0.6; }; diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp index d3b03c6..b8ad295 100644 --- a/perception/src/perception/is_pointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -24,15 +24,13 @@ 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), -threshold_(0.6) +: BT::ConditionNode(xml_tag_name, conf) { config().blackboard->get("node", node_); - getInput("camera_frame", camera_frame_); getInput("low_pointing_limit", low_pointing_limit_); getInput("high_pointing_limit", high_pointing_limit_); - getInput("output_frame", output_frame_); + getInput("threshold", threshold_); RCLCPP_INFO(node_->get_logger(), "Activating perception_people_detection"); node_->add_activation("perception_system/perception_people_detection"); @@ -55,6 +53,8 @@ BT::NodeStatus IsPointing::tick() 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) < @@ -79,7 +79,7 @@ BT::NodeStatus IsPointing::tick() return BT::NodeStatus::FAILURE; } - auto best_detection = detections[0]; + perception_system_interfaces::msg::Detection best_detection = detections[0]; RCLCPP_INFO(node_->get_logger(), "Best detection pointing direction: %d", best_detection.pointing_direction); @@ -93,6 +93,7 @@ BT::NodeStatus IsPointing::tick() 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; } @@ -105,11 +106,11 @@ BT::NodeStatus IsPointing::tick() return BT::NodeStatus::FAILURE; } - // If someones is pointing to a valid direction, return SUCCESS and populate output ports - setOutput("detection", best_detection); + // If someones is pointing to a valid direction, return SUCCESS and populate remaining output ports setOutput("output_frame", output_frame_); - setOutput("pointing_direction", direction); + RCLCPP_INFO(node_->get_logger(), "Someone (%s) is pointing to a valid direction: %d", output_frame_.c_str(), direction); + return BT::NodeStatus::SUCCESS; } From 0beed0ee881fc5a4ee58cb801b43f2c04285c85b Mon Sep 17 00:00:00 2001 From: roi Date: Sat, 30 Nov 2024 18:45:23 +0100 Subject: [PATCH 29/60] IsDetected cleaned --- perception/include/perception/is_detected.hpp | 7 ------- perception/src/perception/is_detected.cpp | 7 ++----- 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/perception/include/perception/is_detected.hpp b/perception/include/perception/is_detected.hpp index 0c5ddaa..54322a1 100644 --- a/perception/include/perception/is_detected.hpp +++ b/perception/include/perception/is_detected.hpp @@ -15,14 +15,8 @@ #ifndef PERCEPTION__ISDETECTED_HPP_ #define PERCEPTION__ISDETECTED_HPP_ -#include -#include -#include -#include - #include #include -#include #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" @@ -65,7 +59,6 @@ class IsDetected : public BT::ConditionNode private: std::shared_ptr node_; - // std::shared_ptr tf_buffer_; std::string interest_, order_, cam_frame_; double threshold_, max_depth_; diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index f3677f6..234f851 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -75,8 +75,7 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - RCLCPP_ERROR(node_->get_logger(), "No detections"); - // RCLCPP_DEBUG(node_->get_logger(), "No detections"); + RCLCPP_ERROR(node_->get_logger(), "No %s detections", interest_.c_str()); return BT::NodeStatus::FAILURE; } @@ -135,10 +134,8 @@ BT::NodeStatus IsDetected::tick() } RCLCPP_INFO(node_->get_logger(), "A %s has been found with the provided conditions", detections[0].class_name.c_str()); - // setOutput("best_detection", detections[0].class_name); - setOutput("best_detection", detections[0]); + setOutput("detection", detections[0]); setOutput("n_dectections", static_cast(detections.size()) + 1); - // setOutput("frames", frames_); setOutput("frame", frames_[0]); // TF frame of the best detected entity // frames_.clear(); From 2c21d91b012eb147e56122089bbc4f337f87cdc9 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 2 Dec 2024 19:20:24 +0100 Subject: [PATCH 30/60] Detection as pointer in bb --- perception/bt_xml/align.xml | 50 +++++++++++++++ perception/include/perception/identify.hpp | 9 ++- perception/include/perception/is_detected.hpp | 2 +- perception/include/perception/is_in_front.hpp | 14 ++++- perception/src/perception/identify.cpp | 36 ++++++----- perception/src/perception/is_detected.cpp | 4 +- perception/src/perception/is_in_front.cpp | 61 ++++++++++++++----- 7 files changed, 135 insertions(+), 41 deletions(-) create mode 100644 perception/bt_xml/align.xml diff --git a/perception/bt_xml/align.xml b/perception/bt_xml/align.xml new file mode 100644 index 0000000..94f85c3 --- /dev/null +++ b/perception/bt_xml/align.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/include/perception/identify.hpp b/perception/include/perception/identify.hpp index ed3b08d..3365a8c 100644 --- a/perception/include/perception/identify.hpp +++ b/perception/include/perception/identify.hpp @@ -52,8 +52,8 @@ class Identify : public BT::ActionNodeBase 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 + BT::InputPort>("detection"), // Detection of the entity to identify. + // If present, it is stored in the blackboard for later use }); } @@ -64,9 +64,8 @@ class Identify : public BT::ActionNodeBase bool detection_at_input_; float confidence_; - perception_system_interfaces::msg::Detection detection_; - -}; + std::shared_ptr detection_; +};; } // namespace perception diff --git a/perception/include/perception/is_detected.hpp b/perception/include/perception/is_detected.hpp index 54322a1..eb4edbe 100644 --- a/perception/include/perception/is_detected.hpp +++ b/perception/include/perception/is_detected.hpp @@ -51,7 +51,7 @@ class IsDetected : public BT::ConditionNode BT::InputPort("confidence"), // Confidence threshold BT::InputPort("order"), // How to sort the detections BT::InputPort("max_depth"), // Max depth to consider - BT::OutputPort("best_detection"), // Best detected entity (first in the list) + 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) }); diff --git a/perception/include/perception/is_in_front.hpp b/perception/include/perception/is_in_front.hpp index fc59480..0356cfe 100644 --- a/perception/include/perception/is_in_front.hpp +++ b/perception/include/perception/is_in_front.hpp @@ -34,6 +34,11 @@ #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 { @@ -52,8 +57,8 @@ class IsInFront : public BT::ConditionNode BT::InputPort("entity_to_identify"), // Name of the entity to check if is in front. Will be used as key in the blackboard // A TF frame with this name will be published BT::InputPort("model"), // YOLO model to use (people, object) - BT::InputPort("detection"), // Detection to check if is in front (optional) - BT::OutputPort("direction") + BT::InputPort>("detection"), // Detection to check if is in front (optional) + BT::OutputPort("direction") }); } @@ -63,7 +68,10 @@ class IsInFront : public BT::ConditionNode double confidence_; std::string entity_; bool detection_at_input_; - perception_system_interfaces::msg::Detection detection_; + std::shared_ptr detection_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; }; diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp index 60d79be..94c2189 100644 --- a/perception/src/perception/identify.cpp +++ b/perception/src/perception/identify.cpp @@ -42,7 +42,6 @@ BT::NodeStatus Identify::tick() { std::vector detections; - // perception_system_interfaces::msg::Detection entity_detection; // if (status() == BT::NodeStatus::IDLE) { detection_at_input_ = true; @@ -64,34 +63,41 @@ Identify::tick() // }); // getInput("detection", entity_detection); // detections = pl::getInstance(node_)->set_features_of_interest(detection_); - config().blackboard->set(entity_, detection_); + config().blackboard->set>(entity_, detection_); + } else { + config().blackboard->get>(entity_, detection_); + RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); } - RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", entity_.c_str()); + // RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", entity_.c_str()); - if (!detection_at_input_) { - config().blackboard->get(entity_, detection_); - RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); - } + // if (!detection_at_input_) { + // config().blackboard->get(entity_, detection_); + // RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); + // } + RCLCPP_INFO(node_->get_logger(), "Getting detection of %s from perception system", entity_.c_str()); pl::getInstance(node_)->update(30); - detections = pl::getInstance(node_)->get_by_features(detection_, confidence_); - - std::sort( - detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - return a.score > b.score; - }); + detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); if (detections.empty()) { - RCLCPP_WARN(node_->get_logger(), "%s NOT detected", entity_.c_str()); + RCLCPP_WARN(node_->get_logger(), "Perception system did not identify %s", entity_.c_str()); return BT::NodeStatus::FAILURE; } - RCLCPP_INFO(node_->get_logger(), "%s detected with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return a.score > b.score; + // }); // Publish the detection + RCLCPP_INFO(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); + + // Update the blackboard with the detection + RCLCPP_INFO(node_->get_logger(), "Updating blackboard with detection of %s", entity_.c_str()); + config().blackboard->set>(entity_, std::make_shared(detections[0])); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index 234f851..3abc901 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -75,7 +75,7 @@ BT::NodeStatus IsDetected::tick() auto detections = pl::getInstance(node_)->get_by_type(interest_); if (detections.empty()) { - RCLCPP_ERROR(node_->get_logger(), "No %s detections", interest_.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "No %s detections", interest_.c_str()); return BT::NodeStatus::FAILURE; } @@ -134,7 +134,7 @@ BT::NodeStatus IsDetected::tick() } RCLCPP_INFO(node_->get_logger(), "A %s has been found with the provided conditions", detections[0].class_name.c_str()); - setOutput("detection", detections[0]); + 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 diff --git a/perception/src/perception/is_in_front.cpp b/perception/src/perception/is_in_front.cpp index 086c3fb..aecb34c 100644 --- a/perception/src/perception/is_in_front.cpp +++ b/perception/src/perception/is_in_front.cpp @@ -20,7 +20,9 @@ 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) +: BT::ConditionNode(xml_tag_name, conf), + tf_buffer_(), + tf_listener_(tf_buffer_) { std::string model; @@ -51,40 +53,69 @@ BT::NodeStatus IsInFront::tick() std::vector detections; if (!detection_at_input_) { - config().blackboard->get(entity_, detection_); + RCLCPP_INFO(node_->get_logger(), "Getting detection from blackboard to check if it is in front"); + config().blackboard->get>(entity_, detection_); } rclcpp::spin_some(node_->get_node_base_interface()); - detections = pl::getInstance(node_)->get_by_features(detection_, confidence_); + detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); + + // Update the blackboard with the detection + RCLCPP_INFO(node_->get_logger(), "Updating blackboard with detection of %s", entity_.c_str()); + config().blackboard->set>(entity_, std::make_shared(detections[0])); if (detections.empty()) { RCLCPP_ERROR(node_->get_logger(), "No detections found"); - setOutput("direction", -1); // If no detections, just turn right bu default + setOutput("direction", -1.0); // If no detections, just turn right bu default return BT::NodeStatus::FAILURE; } - detection_ = detections[0]; + detection_ = std::make_shared(detections[0]); + + geometry_msgs::msg::TransformStamped ps2detection_msg, bf2ps_msg; + tf2::Stamped bf2ps, ps2detection; + ps2detection_msg.transform.translation.x = detection_->center3d.position.x; // Perception system to detection + ps2detection_msg.transform.translation.y = detection_->center3d.position.y; + ps2detection_msg.transform.translation.z = detection_->center3d.position.z; + ps2detection_msg.header.frame_id = detection_->header.frame_id; + + bf2ps_msg = tf_buffer_.lookupTransform("base_footprint", detection_->header.frame_id, tf2::TimePointZero); + + tf2::fromMsg(bf2ps_msg, bf2ps); + tf2::fromMsg(ps2detection_msg, ps2detection); + tf2::Transform bf2detection = bf2ps * ps2detection; + + // double yaw = tf2::getYaw(bf2detection.getRotation()); + double yaw = std::atan2(bf2detection.getOrigin().y(), bf2detection.getOrigin().x()); - // 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); yaw = yaw * 180.0 / M_PI; + RCLCPP_DEBUG(node_->get_logger(), "Detection at (%.2f, %.2f, %.2f) from %s", + detection_->center3d.position.x, + detection_->center3d.position.y, + detection_->center3d.position.z, + detection_->header.frame_id.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Detection at (%.2f, %.2f, %.2f) from base_footprint. YAW: %.2f degrees", + bf2detection.getOrigin().x(), + bf2detection.getOrigin().y(), + bf2detection.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) { - setOutput("direction", 1); // Left + RCLCPP_INFO(node_->get_logger(), "Detection is at %.2f degrees to the left", yaw); + setOutput("direction", 1.0); // Left } else { - setOutput("direction", -1); // Right + RCLCPP_INFO(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 - pl::getInstance(node_)->publishTF_EKF(detection_, entity_, true); - setOutput("direction", 0); + // The detection is in front. Publish the detection + RCLCPP_INFO(node_->get_logger(), "Detection is in front"); + pl::getInstance(node_)->publishTF_EKF(*detection_, entity_, true); + setOutput("direction", 0.0); return BT::NodeStatus::SUCCESS; From 19692e288fe6f4dc7025acca029ff334381c7588 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 2 Dec 2024 19:39:25 +0100 Subject: [PATCH 31/60] IsPointing uptated --- perception/include/perception/is_pointing.hpp | 2 +- perception/src/perception/is_pointing.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/include/perception/is_pointing.hpp b/perception/include/perception/is_pointing.hpp index 78aa7ff..14d6d71 100644 --- a/perception/include/perception/is_pointing.hpp +++ b/perception/include/perception/is_pointing.hpp @@ -50,7 +50,7 @@ class IsPointing : public BT::ConditionNode BT::InputPort("low_pointing_limit"), BT::InputPort("high_pointing_limit"), BT::InputPort("threshold"), - BT::OutputPort("detection"), + BT::OutputPort>("detection"), BT::OutputPort("pointing_direction"), BT::OutputPort("output_frame") }); diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp index b8ad295..0a401aa 100644 --- a/perception/src/perception/is_pointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -79,22 +79,22 @@ BT::NodeStatus IsPointing::tick() return BT::NodeStatus::FAILURE; } - perception_system_interfaces::msg::Detection best_detection = detections[0]; + auto best_detection = std::make_shared(detections[0]); RCLCPP_INFO(node_->get_logger(), "Best detection pointing direction: %d", - best_detection.pointing_direction); + 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) { + 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) { + if (pl::getInstance(node_)->publishTF_EKF(*best_detection, output_frame_, true) == -1) { return BT::NodeStatus::FAILURE; } break; From 8c731161e9b408fc8f33dd22bd229432b6786abd Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 3 Dec 2024 12:05:05 +0100 Subject: [PATCH 32/60] refined --- motion/src/motion/base/spin.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/motion/src/motion/base/spin.cpp b/motion/src/motion/base/spin.cpp index 21ac6ac..452b26b 100644 --- a/motion/src/motion/base/spin.cpp +++ b/motion/src/motion/base/spin.cpp @@ -28,7 +28,7 @@ Spin::Spin(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) BT::NodeStatus Spin::tick() { - RCLCPP_DEBUG(node_->get_logger(), "Spin ticked"); + RCLCPP_DEBUG(node_->get_logger(), "SPIN"); getInput("angle", angle_); getInput("speed", speed_); @@ -57,7 +57,9 @@ BT::NodeStatus Spin::tick() if (forever_) { RCLCPP_DEBUG(node_->get_logger(), "Spinning forever"); - vel_pub_->publish(cmd_vel); + 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_); From 8d695d5830be0806e435db20e3eed1315a0afad4 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 3 Dec 2024 12:06:59 +0100 Subject: [PATCH 33/60] XML adapted to new perception nodes --- perception/bt_xml/align.xml | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/perception/bt_xml/align.xml b/perception/bt_xml/align.xml index 94f85c3..97ce2c4 100644 --- a/perception/bt_xml/align.xml +++ b/perception/bt_xml/align.xml @@ -8,20 +8,19 @@ - - + - - + + - + @@ -34,11 +33,8 @@ - - - From 0a15e7945657a1c9ae2260f2f6b1ffe36e51f9f6 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 3 Dec 2024 12:07:10 +0100 Subject: [PATCH 34/60] exlude _*.xml --- .gitignore | 3 +- perception/bt_xml/align.xml | 21 +++-- perception/config/perception.yaml | 6 +- perception/include/perception/identify.hpp | 1 - perception/include/perception/is_in_front.hpp | 10 +-- perception/src/main_perception.cpp | 2 +- perception/src/perception/identify.cpp | 87 +++++-------------- perception/src/perception/is_detected.cpp | 12 +-- perception/src/perception/is_in_front.cpp | 77 +++------------- 9 files changed, 66 insertions(+), 153 deletions(-) diff --git a/.gitignore b/.gitignore index 8ce0960..6063d18 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .gitingore _*.hpp _*.cpp -_*.txt \ No newline at end of file +_*.txt +_*.xml \ No newline at end of file diff --git a/perception/bt_xml/align.xml b/perception/bt_xml/align.xml index 97ce2c4..99b2b08 100644 --- a/perception/bt_xml/align.xml +++ b/perception/bt_xml/align.xml @@ -8,16 +8,20 @@ - - - - - - - + + + + + + + + + + + @@ -41,6 +45,9 @@ + + If false (default), the Subtree has an isolated blackboard and needs port remapping + diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml index 0fd83d4..61d8cc0 100644 --- a/perception/config/perception.yaml +++ b/perception/config/perception.yaml @@ -1,10 +1,12 @@ perception_node: ros__parameters: use_sim_time: True - # tf_frame_camera: head_front_camera_optical_frame - bt_xml_file: identify_person.xml + tf_frame_camera: head_front_camera_optical_frame # oak_rgb_camera_optical_frame / head_front_camera_optical_frame + bt_xml_file: align.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/identify.hpp b/perception/include/perception/identify.hpp index 3365a8c..9a097d0 100644 --- a/perception/include/perception/identify.hpp +++ b/perception/include/perception/identify.hpp @@ -61,7 +61,6 @@ class Identify : public BT::ActionNodeBase std::shared_ptr node_; std::string entity_; - bool detection_at_input_; float confidence_; std::shared_ptr detection_; diff --git a/perception/include/perception/is_in_front.hpp b/perception/include/perception/is_in_front.hpp index 0356cfe..a512d02 100644 --- a/perception/include/perception/is_in_front.hpp +++ b/perception/include/perception/is_in_front.hpp @@ -53,11 +53,7 @@ class IsInFront : public BT::ConditionNode { return BT::PortsList( { - BT::InputPort("confidence"), // Confidence threshold - BT::InputPort("entity_to_identify"), // Name of the entity to check if is in front. Will be used as key in the blackboard - // A TF frame with this name will be published - BT::InputPort("model"), // YOLO model to use (people, object) - BT::InputPort>("detection"), // Detection to check if is in front (optional) + BT::InputPort("entity_to_identify"), // Name of the tf frame of the entity to check if is in front BT::OutputPort("direction") }); } @@ -65,10 +61,8 @@ class IsInFront : public BT::ConditionNode private: std::shared_ptr node_; - double confidence_; + // double confidence_; std::string entity_; - bool detection_at_input_; - std::shared_ptr detection_; tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/perception/src/main_perception.cpp b/perception/src/main_perception.cpp index 01d27e2..342190b 100644 --- a/perception/src/main_perception.cpp +++ b/perception/src/main_perception.cpp @@ -60,7 +60,7 @@ int main(int argc, char * argv[]) auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); - rclcpp::Rate rate(30); + rclcpp::Rate rate(15); bool finish = false; while (!finish && rclcpp::ok()) { diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp index 94c2189..5807e8b 100644 --- a/perception/src/perception/identify.cpp +++ b/perception/src/perception/identify.cpp @@ -25,7 +25,7 @@ Identify::Identify( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), entity_(""), - confidence_(0.5) + confidence_(0.2) { config().blackboard->get("node", node_); @@ -34,6 +34,8 @@ Identify::Identify( if (!getInput("confidence", confidence_)) { RCLCPP_WARN(node_->get_logger(), "No confidence provided. Using default value %f", confidence_); + } else { + RCLCPP_INFO(node_->get_logger(), "Confidence threshold: %.2f", confidence_); } } @@ -41,73 +43,32 @@ Identify::Identify( BT::NodeStatus Identify::tick() { + RCLCPP_DEBUG(node_->get_logger(), "IDENTIFY"); std::vector detections; - // if (status() == BT::NodeStatus::IDLE) { - detection_at_input_ = true; - if (!getInput("detection", detection_)) { - RCLCPP_DEBUG(node_->get_logger(), "No detection at input"); - detection_at_input_ = false; - } - // } - - try - { - - if (detection_at_input_) { // Configure perception system to track the specified person - RCLCPP_INFO(node_->get_logger(), "Storing detection of %s", entity_.c_str()); - // detections = pl::getInstance(node_)->get_by_type("person"); - // std::sort( - // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - // return a.center3d.position.z < b.center3d.position.z; - // }); - // getInput("detection", entity_detection); - // detections = pl::getInstance(node_)->set_features_of_interest(detection_); - config().blackboard->set>(entity_, detection_); - } else { - config().blackboard->get>(entity_, detection_); - RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); - } - - // RCLCPP_INFO(node_->get_logger(), "Getting detection of %s", entity_.c_str()); - - // if (!detection_at_input_) { - // config().blackboard->get(entity_, detection_); - // RCLCPP_INFO(node_->get_logger(), "Detection of %s retrieved", entity_.c_str()); - // } - - RCLCPP_INFO(node_->get_logger(), "Getting detection of %s from perception system", entity_.c_str()); - pl::getInstance(node_)->update(30); - detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); - - if (detections.empty()) - { - RCLCPP_WARN(node_->get_logger(), "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_INFO(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); - pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); - - // Update the blackboard with the detection - RCLCPP_INFO(node_->get_logger(), "Updating blackboard with detection of %s", entity_.c_str()); - config().blackboard->set>(entity_, std::make_shared(detections[0])); - - return BT::NodeStatus::SUCCESS; + if (!getInput("detection", detection_)) { + RCLCPP_ERROR(node_->get_logger(), "No detection at input"); + return BT::NodeStatus::FAILURE; } - catch(const std::exception& ex) - { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + + pl::getInstance(node_)->update(30); + detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); + + if (detections.empty()) { + RCLCPP_WARN(node_->get_logger(), "Perception system did not identify %s", entity_.c_str()); return BT::NodeStatus::FAILURE; } - - return BT::NodeStatus::RUNNING; + + std::sort( + detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + return a.score > b.score; + }); + + // Publish the detection + RCLCPP_INFO(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); + pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); + + return BT::NodeStatus::SUCCESS; } } // namespace perception diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index 3abc901..822df9a 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -31,10 +31,10 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura getInput("model", model); if (model == "people") { - RCLCPP_INFO(node_->get_logger(), "Activating perception_people_detection"); + RCLCPP_DEBUG(node_->get_logger(), "Activating perception_people_detection"); node_->add_activation("perception_system/perception_people_detection"); } else if (model == "object") { - RCLCPP_INFO(node_->get_logger(), "Activating perception_object_detection"); + 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()); @@ -48,7 +48,7 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura getInput("order", order_); getInput("max_depth", max_depth_); - RCLCPP_INFO(node_->get_logger(), "Interest: %s", interest_.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Interest: %s", interest_.c_str()); frames_.clear(); } @@ -79,7 +79,7 @@ BT::NodeStatus IsDetected::tick() return BT::NodeStatus::FAILURE; } - RCLCPP_INFO(node_->get_logger(), "Processing %ld detections...", detections.size()); + 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 @@ -121,7 +121,7 @@ BT::NodeStatus IsDetected::tick() 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()); + RCLCPP_DEBUG(node_->get_logger(), "Publishing TF %s", tf_name.c_str()); ++it; ++entity_counter; } @@ -140,7 +140,7 @@ BT::NodeStatus IsDetected::tick() // frames_.clear(); - RCLCPP_INFO(node_->get_logger(), "Detections published"); + RCLCPP_DEBUG(node_->get_logger(), "Detections published"); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/is_in_front.cpp b/perception/src/perception/is_in_front.cpp index aecb34c..5836e41 100644 --- a/perception/src/perception/is_in_front.cpp +++ b/perception/src/perception/is_in_front.cpp @@ -24,81 +24,31 @@ IsInFront::IsInFront(const std::string & xml_tag_name, const BT::NodeConfigurati tf_buffer_(), tf_listener_(tf_buffer_) { - std::string model; - config().blackboard->get("node", node_); - getInput("confidence", confidence_); - getInput("model", model); getInput("entity_to_identify", entity_); - - detection_at_input_ = true; - if (!getInput("detection", detection_)) { - detection_at_input_ = false; - } - - if (model == "people") { - node_->add_activation("perception_system/perception_people_detection"); - } else if (model == "object") { - 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"); - } } BT::NodeStatus IsInFront::tick() { - RCLCPP_DEBUG(node_->get_logger(), "IsInFront ticked"); - - std::vector detections; - - if (!detection_at_input_) { - RCLCPP_INFO(node_->get_logger(), "Getting detection from blackboard to check if it is in front"); - config().blackboard->get>(entity_, detection_); - } + RCLCPP_DEBUG(node_->get_logger(), "IS_IN_FRONT"); rclcpp::spin_some(node_->get_node_base_interface()); - detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); - - // Update the blackboard with the detection - RCLCPP_INFO(node_->get_logger(), "Updating blackboard with detection of %s", entity_.c_str()); - config().blackboard->set>(entity_, std::make_shared(detections[0])); - - if (detections.empty()) { - RCLCPP_ERROR(node_->get_logger(), "No detections found"); - setOutput("direction", -1.0); // If no detections, just turn right bu default - return BT::NodeStatus::FAILURE; - } - - detection_ = std::make_shared(detections[0]); - - geometry_msgs::msg::TransformStamped ps2detection_msg, bf2ps_msg; - tf2::Stamped bf2ps, ps2detection; - ps2detection_msg.transform.translation.x = detection_->center3d.position.x; // Perception system to detection - ps2detection_msg.transform.translation.y = detection_->center3d.position.y; - ps2detection_msg.transform.translation.z = detection_->center3d.position.z; - ps2detection_msg.header.frame_id = detection_->header.frame_id; - - bf2ps_msg = tf_buffer_.lookupTransform("base_footprint", detection_->header.frame_id, tf2::TimePointZero); + geometry_msgs::msg::TransformStamped base2entity_msg; + tf2::Stamped base2entity; - tf2::fromMsg(bf2ps_msg, bf2ps); - tf2::fromMsg(ps2detection_msg, ps2detection); - tf2::Transform bf2detection = bf2ps * ps2detection; - - // double yaw = tf2::getYaw(bf2detection.getRotation()); - double yaw = std::atan2(bf2detection.getOrigin().y(), bf2detection.getOrigin().x()); + 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 %s", - detection_->center3d.position.x, - detection_->center3d.position.y, - detection_->center3d.position.z, - detection_->header.frame_id.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Detection at (%.2f, %.2f, %.2f) from base_footprint. YAW: %.2f degrees", - bf2detection.getOrigin().x(), - bf2detection.getOrigin().y(), - bf2detection.getOrigin().z(), + 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 @@ -113,8 +63,7 @@ BT::NodeStatus IsInFront::tick() } // The detection is in front. Publish the detection - RCLCPP_INFO(node_->get_logger(), "Detection is in front"); - pl::getInstance(node_)->publishTF_EKF(*detection_, entity_, true); + RCLCPP_INFO(node_->get_logger(), "Detection is in front!"); setOutput("direction", 0.0); return BT::NodeStatus::SUCCESS; From 4a519ddb5f2843b0441bbb172dc8bfd819a8ad71 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 3 Dec 2024 13:01:42 +0100 Subject: [PATCH 35/60] minor --- perception/include/perception/is_in_front.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/include/perception/is_in_front.hpp b/perception/include/perception/is_in_front.hpp index a512d02..2e8213a 100644 --- a/perception/include/perception/is_in_front.hpp +++ b/perception/include/perception/is_in_front.hpp @@ -61,7 +61,6 @@ class IsInFront : public BT::ConditionNode private: std::shared_ptr node_; - // double confidence_; std::string entity_; tf2::BufferCore tf_buffer_; From 741bb02b1cbbb219870152cdd991dd089c26adf3 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 4 Dec 2024 17:40:49 +0100 Subject: [PATCH 36/60] N --- perception/CMakeLists.txt | 1 + .../perception/get_detection_from_tf.hpp | 73 +++++++++++++++++++ .../src/perception/get_detection_from_tf.cpp | 62 ++++++++++++++++ 3 files changed, 136 insertions(+) create mode 100644 perception/include/perception/get_detection_from_tf.hpp create mode 100644 perception/src/perception/get_detection_from_tf.cpp diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index 3759f90..a782153 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -53,6 +53,7 @@ set(plugin_sources src/perception/identify.cpp src/perception/get_angle.cpp src/perception/is_in_front.cpp + src/perception/get_detection_from_tf.cpp ) set(plugin_libs "") 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/src/perception/get_detection_from_tf.cpp b/perception/src/perception/get_detection_from_tf.cpp new file mode 100644 index 0000000..9e6ea15 --- /dev/null +++ b/perception/src/perception/get_detection_from_tf.cpp @@ -0,0 +1,62 @@ +// 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"); + + auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); + + 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"); +} + From e0083c347e16d3b230f049f7b1c131dfd669fb6f Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 5 Dec 2024 12:14:29 +0100 Subject: [PATCH 37/60] unnecesary port --- perception/include/perception/is_detected.hpp | 1 - perception/src/perception/is_detected.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/perception/include/perception/is_detected.hpp b/perception/include/perception/is_detected.hpp index eb4edbe..f719902 100644 --- a/perception/include/perception/is_detected.hpp +++ b/perception/include/perception/is_detected.hpp @@ -46,7 +46,6 @@ class IsDetected : public BT::ConditionNode { BT::InputPort("max_entities"), // Max number of entities to consider BT::InputPort("model"), // YOLO model (object or people) - BT::InputPort("cam_frame"), // Camera frame BT::InputPort("interest"), // What to look for BT::InputPort("confidence"), // Confidence threshold BT::InputPort("order"), // How to sort the detections diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index 822df9a..1ae1fba 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -42,7 +42,6 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura } getInput("interest", interest_); - getInput("cam_frame", cam_frame_); getInput("confidence", threshold_); getInput("max_entities", max_entities_); getInput("order", order_); @@ -57,17 +56,16 @@ BT::NodeStatus IsDetected::tick() { rclcpp::spin_some(node_->get_node_base_interface()); // getInput("interest", interest_); - // getInput("cam_frame", cam_frame_); // getInput("confidence", threshold_); // getInput("max_entities", max_entities_); // getInput("order", order_); // getInput("max_depth", max_depth_); - if (status() == BT::NodeStatus::IDLE) { - RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); - } + // if (status() == BT::NodeStatus::IDLE) { + // RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); + // } - RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked"); + RCLCPP_DEBUG(node_->get_logger(), "IS_DETECTED"); pl::getInstance(node_)->set_interest(interest_, true); pl::getInstance(node_)->update(35); From b0ddf1ae6b28842022b21bbbec54a94fe215171b Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 9 Dec 2024 16:42:16 +0100 Subject: [PATCH 38/60] minor --- perception/src/perception/is_detected.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index 1ae1fba..fed2fdf 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -54,6 +54,7 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura BT::NodeStatus IsDetected::tick() { + RCLCPP_DEBUG(node_->get_logger(), "IS_DETECTED"); rclcpp::spin_some(node_->get_node_base_interface()); // getInput("interest", interest_); // getInput("confidence", threshold_); @@ -65,8 +66,6 @@ BT::NodeStatus IsDetected::tick() // RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); // } - RCLCPP_DEBUG(node_->get_logger(), "IS_DETECTED"); - pl::getInstance(node_)->set_interest(interest_, true); pl::getInstance(node_)->update(35); From 414b97ecd521d73fcc80e0b83bcba4c5143bff37 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 9 Dec 2024 16:42:25 +0100 Subject: [PATCH 39/60] minor --- perception/src/perception/is_pointing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/src/perception/is_pointing.cpp b/perception/src/perception/is_pointing.cpp index 0a401aa..658e300 100644 --- a/perception/src/perception/is_pointing.cpp +++ b/perception/src/perception/is_pointing.cpp @@ -38,6 +38,7 @@ IsPointing::IsPointing(const std::string & xml_tag_name, const BT::NodeConfigura 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"); From d7f109a36abb6d18b059031b176039b9895239df Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 12:32:24 +0100 Subject: [PATCH 40/60] minor --- hri/src/hri/dialog/speak.cpp | 3 ++- .../include/motion/navigation/navigate_to.hpp | 9 ++++++--- motion/src/motion/navigation/navigate_to.cpp | 18 +++++++++++++----- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/hri/src/hri/dialog/speak.cpp b/hri/src/hri/dialog/speak.cpp index e1104b8..e620610 100644 --- a/hri/src/hri/dialog/speak.cpp +++ b/hri/src/hri/dialog/speak.cpp @@ -61,7 +61,8 @@ Speak::swap_placeholders(std::string text, std::vector elems) void Speak::on_tick() { - RCLCPP_DEBUG(node_->get_logger(), "Speak ticked"); + RCLCPP_INFO(node_->get_logger(), "SPEAK"); + rclcpp::spin_some(node_->get_node_base_interface()); goal_ = audio_common_msgs::action::TTS::Goal(); diff --git a/motion/include/motion/navigation/navigate_to.hpp b/motion/include/motion/navigation/navigate_to.hpp index 1e9e849..6875957 100644 --- a/motion/include/motion/navigation/navigate_to.hpp +++ b/motion/include/motion/navigation/navigate_to.hpp @@ -70,9 +70,12 @@ class NavigateTo : public motion::BtActionNode< rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_executor_; - tf2::BufferCore tf_buffer_; - tf2_ros::TransformListener tf_listener_; - + // tf2::BufferCore tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + bool will_finish_{true}; }; diff --git a/motion/src/motion/navigation/navigate_to.cpp b/motion/src/motion/navigation/navigate_to.cpp index 14531a3..059a7b7 100644 --- a/motion/src/motion/navigation/navigate_to.cpp +++ b/motion/src/motion/navigation/navigate_to.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "motion/navigation/navigate_to.hpp" +#include namespace navigation { @@ -21,11 +22,14 @@ 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_) + 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() @@ -44,10 +48,14 @@ void NavigateTo::on_tick() if (tf_frame.length() > 0) { // There is a TF to go, ignore coordinates RCLCPP_INFO(node_->get_logger(), "Transforming map to %s", tf_frame.c_str()); try { - map_to_goal = tf_buffer_.lookupTransform("map", tf_frame, tf2::TimePointZero); + 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(), "Could not transform %s to %s: %s", "map", tf_frame.c_str(), ex.what()); + node_->get_logger(), "Could not transform map to %s: %s", tf_frame.c_str(), ex.what()); setStatus(BT::NodeStatus::FAILURE); return; } From 2545293c96a7aef5735f604568ce81a9a9af376d Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 12:32:41 +0100 Subject: [PATCH 41/60] new BT node added --- perception/CMakeLists.txt | 1 + perception/include/perception/is_in_view.hpp | 75 ++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100644 perception/include/perception/is_in_view.hpp diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index a782153..f529657 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -53,6 +53,7 @@ set(plugin_sources 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 ) diff --git a/perception/include/perception/is_in_view.hpp b/perception/include/perception/is_in_view.hpp new file mode 100644 index 0000000..f6e4199 --- /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("frame") // Name of the tf frame of the entity to check if is in front + }); + } + +private: + std::shared_ptr node_; + + std::string frame_; + + tf2::BufferCore tf_buffer_; + tf2_ros::TransformListener tf_listener_; + +}; + +} // namespace perception + +#endif // PERCEPTION_IS_IN_VIEW_HPP_ From 2d483617463ebac826b7e5bf60bfb745e35b2434 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 12:32:55 +0100 Subject: [PATCH 42/60] xml --- perception/bt_xml/detect_chair.xml | 3 +- perception/bt_xml/detect_person.xml | 44 +++++++++++++++++++++++++++ perception/bt_xml/detect_person2.xml | 45 ++++++++++++++++++++++++++++ 3 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 perception/bt_xml/detect_person.xml create mode 100644 perception/bt_xml/detect_person2.xml diff --git a/perception/bt_xml/detect_chair.xml b/perception/bt_xml/detect_chair.xml index 56448d6..8fdec1e 100644 --- a/perception/bt_xml/detect_chair.xml +++ b/perception/bt_xml/detect_chair.xml @@ -4,7 +4,7 @@ - + @@ -18,7 +18,6 @@ - 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 + + + + + + + + + + + From 3596173f4b00ac4ee63fde9336e27bd963605d04 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 12:33:15 +0100 Subject: [PATCH 43/60] refined --- perception/config/perception.yaml | 2 +- perception/include/perception/is_detected.hpp | 3 +- perception/src/perception/get_angle.cpp | 2 +- .../src/perception/get_detection_from_tf.cpp | 9 ++- perception/src/perception/identify.cpp | 14 +++-- perception/src/perception/is_detected.cpp | 11 +++- perception/src/perception/is_in_front.cpp | 7 +-- perception/src/perception/is_in_view.cpp | 58 +++++++++++++++++++ 8 files changed, 90 insertions(+), 16 deletions(-) create mode 100644 perception/src/perception/is_in_view.cpp diff --git a/perception/config/perception.yaml b/perception/config/perception.yaml index 61d8cc0..055cfee 100644 --- a/perception/config/perception.yaml +++ b/perception/config/perception.yaml @@ -2,7 +2,7 @@ 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: align.xml + bt_xml_file: detect_person.xml plugins: - is_in_front_bt_node - is_detected_bt_node diff --git a/perception/include/perception/is_detected.hpp b/perception/include/perception/is_detected.hpp index f719902..bbdb53c 100644 --- a/perception/include/perception/is_detected.hpp +++ b/perception/include/perception/is_detected.hpp @@ -50,6 +50,7 @@ class IsDetected : public BT::ConditionNode 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) @@ -59,7 +60,7 @@ class IsDetected : public BT::ConditionNode private: std::shared_ptr node_; - std::string interest_, order_, cam_frame_; + std::string interest_, order_, frame_name_; double threshold_, max_depth_; int max_entities_; std::int64_t person_id_; diff --git a/perception/src/perception/get_angle.cpp b/perception/src/perception/get_angle.cpp index 805774a..0830e91 100644 --- a/perception/src/perception/get_angle.cpp +++ b/perception/src/perception/get_angle.cpp @@ -43,13 +43,13 @@ GetAngle::GetAngle(const std::string & xml_tag_name, const BT::NodeConfiguration 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); - rclcpp::spin_some(node_->get_node_base_interface()); detections = pl::getInstance(node_)->get_by_features(detection, confidence_); diff --git a/perception/src/perception/get_detection_from_tf.cpp b/perception/src/perception/get_detection_from_tf.cpp index 9e6ea15..367a5c7 100644 --- a/perception/src/perception/get_detection_from_tf.cpp +++ b/perception/src/perception/get_detection_from_tf.cpp @@ -35,8 +35,15 @@ BT::NodeStatus GetDetectionFromTF::tick() { RCLCPP_DEBUG(node_->get_logger(), "GET_DETECTION_FROM_TF"); + rclcpp::spin_some(node_->get_node_base_interface()); - auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); + 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); diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp index 5807e8b..eed1118 100644 --- a/perception/src/perception/identify.cpp +++ b/perception/src/perception/identify.cpp @@ -44,6 +44,7 @@ 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_)) { @@ -52,6 +53,7 @@ Identify::tick() } pl::getInstance(node_)->update(30); + pl::getInstance(node_)->set_interest("person", true); detections = pl::getInstance(node_)->get_by_features(*detection_, confidence_); if (detections.empty()) { @@ -59,15 +61,15 @@ Identify::tick() return BT::NodeStatus::FAILURE; } - std::sort( - detections.begin(), detections.end(), [this](const auto & a, const auto & b) { - return a.score > b.score; - }); + // std::sort( + // detections.begin(), detections.end(), [this](const auto & a, const auto & b) { + // return a.score > b.score; + // }); // Publish the detection - RCLCPP_INFO(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); + RCLCPP_DEBUG(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); pl::getInstance(node_)->publishTF_EKF(detections[0], entity_, true); - + detections.clear(); return BT::NodeStatus::SUCCESS; } diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index fed2fdf..7de3ff7 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -46,6 +46,7 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura 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()); @@ -113,12 +114,18 @@ BT::NodeStatus IsDetected::tick() } else { std::string tf_name; - tf_name = detection.class_name + "_" + std::to_string(entity_counter + 1); + + 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_DEBUG(node_->get_logger(), "Publishing TF %s", tf_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "Publishing TF %s", tf_name.c_str()); ++it; ++entity_counter; } diff --git a/perception/src/perception/is_in_front.cpp b/perception/src/perception/is_in_front.cpp index 5836e41..078c1e4 100644 --- a/perception/src/perception/is_in_front.cpp +++ b/perception/src/perception/is_in_front.cpp @@ -31,7 +31,6 @@ IsInFront::IsInFront(const std::string & xml_tag_name, const BT::NodeConfigurati 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; @@ -53,17 +52,17 @@ BT::NodeStatus IsInFront::tick() if (std::abs((yaw)) > 5.0) { // If the angle is greater than 5 degrees, the detection is not in front if (yaw > 0) { - RCLCPP_INFO(node_->get_logger(), "Detection is at %.2f degrees to the left", yaw); + RCLCPP_DEBUG(node_->get_logger(), "Detection is at %.2f degrees to the left", yaw); setOutput("direction", 1.0); // Left } else { - RCLCPP_INFO(node_->get_logger(), "Detection is at %.2f degrees to the right", yaw); + 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_INFO(node_->get_logger(), "Detection is in front!"); + RCLCPP_DEBUG(node_->get_logger(), "Detection is in front!"); setOutput("direction", 0.0); return BT::NodeStatus::SUCCESS; diff --git a/perception/src/perception/is_in_view.cpp b/perception/src/perception/is_in_view.cpp new file mode 100644 index 0000000..f227e6b --- /dev/null +++ b/perception/src/perception/is_in_view.cpp @@ -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. + +#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("frame", frame_); +} +BT::NodeStatus IsInView::tick() +{ + RCLCPP_DEBUG(node_->get_logger(), "IS_IN_VIEW"); + rclcpp::spin_some(node_->get_node_base_interface()); + + auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); + + pl::getInstance(node_)->update(30); + auto detections = pl::getInstance(node_)->get_detection_at(base2entity_msg); + RCLCPP_DEBUG(node_->get_logger(), "Detection obtained from TF"); + + if (detections.empty()) { + RCLCPP_INFO(node_->get_logger(), "Detection not in view"); + 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; + } + + return BT::NodeStatus::SUCCESS; + +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("IsInView"); +} From f39da9d8c34c5692dec049ae0ab71048ee8c7e2c Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 15:31:42 +0100 Subject: [PATCH 44/60] minor --- perception/include/perception/is_in_front.hpp | 2 -- perception/src/perception/is_detected.cpp | 9 --------- 2 files changed, 11 deletions(-) diff --git a/perception/include/perception/is_in_front.hpp b/perception/include/perception/is_in_front.hpp index 2e8213a..d29daef 100644 --- a/perception/include/perception/is_in_front.hpp +++ b/perception/include/perception/is_in_front.hpp @@ -15,8 +15,6 @@ #ifndef PERCEPTION_IS_IN_FRONT_HPP_ #define PERCEPTION_IS_IN_FRONT_HPP_ -#include - #include #include #include diff --git a/perception/src/perception/is_detected.cpp b/perception/src/perception/is_detected.cpp index 7de3ff7..ca87097 100644 --- a/perception/src/perception/is_detected.cpp +++ b/perception/src/perception/is_detected.cpp @@ -57,15 +57,6 @@ BT::NodeStatus IsDetected::tick() { RCLCPP_DEBUG(node_->get_logger(), "IS_DETECTED"); rclcpp::spin_some(node_->get_node_base_interface()); - // getInput("interest", interest_); - // getInput("confidence", threshold_); - // getInput("max_entities", max_entities_); - // getInput("order", order_); - // getInput("max_depth", max_depth_); - - // if (status() == BT::NodeStatus::IDLE) { - // RCLCPP_DEBUG(node_->get_logger(), "IsDetected ticked while IDLE"); - // } pl::getInstance(node_)->set_interest(interest_, true); pl::getInstance(node_)->update(35); From 0a5bdb5657e704c9f142875287a700770e4522c7 Mon Sep 17 00:00:00 2001 From: roi Date: Thu, 12 Dec 2024 17:44:58 +0100 Subject: [PATCH 45/60] attention nodes untested --- attention/CMakeLists.txt | 63 +++++++++++++++ attention/README.md | 2 + attention/config/attention.yaml | 12 +++ .../include/attention/activate_attention.hpp | 59 ++++++++++++++ .../attention/deactivate_attention.hpp | 58 ++++++++++++++ attention/launch/attention.launch.py | 50 ++++++++++++ attention/package.xml | 25 ++++++ .../src/attention/activate_attention.cpp | 62 +++++++++++++++ .../src/attention/deactivate_attention.cpp | 62 +++++++++++++++ attention/src/main_attention.cpp | 76 +++++++++++++++++++ attention/thirdparty.repos | 9 +++ 11 files changed, 478 insertions(+) create mode 100644 attention/CMakeLists.txt create mode 100644 attention/README.md create mode 100644 attention/config/attention.yaml create mode 100644 attention/include/attention/activate_attention.hpp create mode 100644 attention/include/attention/deactivate_attention.hpp create mode 100644 attention/launch/attention.launch.py create mode 100644 attention/package.xml create mode 100644 attention/src/attention/activate_attention.cpp create mode 100644 attention/src/attention/deactivate_attention.cpp create mode 100644 attention/src/main_attention.cpp create mode 100644 attention/thirdparty.repos 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/config/attention.yaml b/attention/config/attention.yaml new file mode 100644 index 0000000..055cfee --- /dev/null +++ b/attention/config/attention.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/attention/include/attention/activate_attention.hpp b/attention/include/attention/activate_attention.hpp new file mode 100644 index 0000000..19da830 --- /dev/null +++ b/attention/include/attention/activate_attention.hpp @@ -0,0 +1,59 @@ +// 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") + }); + } + +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..74a3a02 --- /dev/null +++ b/attention/launch/attention.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/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..8b2672a --- /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(), "ACTIVATE_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..342190b --- /dev/null +++ b/attention/src/main_attention.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/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 From e0774d854b51790ce43d28a93e9f6f4b7ce80898 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 13 Dec 2024 10:34:45 +0100 Subject: [PATCH 46/60] nodes --- attention/bt_xml/attend.xml | 26 +++++++++ attention/config/attention.yaml | 13 ++--- .../include/attention/activate_attention.hpp | 3 +- attention/launch/attention.launch.py | 12 ++-- .../src/attention/activate_attention.cpp | 2 +- attention/src/main_attention.cpp | 5 +- .../perception/get_detection_from_bb.hpp | 56 +++++++++++++++++++ .../src/perception/get_detection_from_bb.cpp | 51 +++++++++++++++++ 8 files changed, 147 insertions(+), 21 deletions(-) create mode 100644 attention/bt_xml/attend.xml create mode 100644 perception/include/perception/get_detection_from_bb.hpp create mode 100644 perception/src/perception/get_detection_from_bb.cpp 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 index 055cfee..fdd656e 100644 --- a/attention/config/attention.yaml +++ b/attention/config/attention.yaml @@ -1,12 +1,7 @@ -perception_node: +attention_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 + bt_xml_file: attend.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 + - 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 index 19da830..055f36c 100644 --- a/attention/include/attention/activate_attention.hpp +++ b/attention/include/attention/activate_attention.hpp @@ -41,7 +41,8 @@ class ActivateAttention : public BT::ActionNodeBase { return BT::PortsList( { - BT::InputPort("what") + BT::InputPort("what"), + BT::InputPort("frame_id"), }); } diff --git a/attention/launch/attention.launch.py b/attention/launch/attention.launch.py index 74a3a02..aa61df7 100644 --- a/attention/launch/attention.launch.py +++ b/attention/launch/attention.launch.py @@ -23,19 +23,17 @@ def generate_launch_description(): # Get the launch directory - pkg_dir = get_package_share_directory('perception_bt_nodes') + pkg_dir = get_package_share_directory('attention_bt_nodes') - params_file = os.path.join(pkg_dir, 'config', 'perception.yaml') - # print('params_file: ', params_file) + params_file = os.path.join(pkg_dir, 'config', 'attention.yaml') with open(params_file, 'r') as f: - params = yaml.safe_load(f)['perception_node']['ros__parameters'] - # print(params) + params = yaml.safe_load(f)['attention_node']['ros__parameters'] ld = LaunchDescription() nav_cmd = Node( - package='perception_bt_nodes', - executable='perception_test', + package='attention_bt_nodes', + executable='attention_test', output='screen', remappings=[ ], diff --git a/attention/src/attention/activate_attention.cpp b/attention/src/attention/activate_attention.cpp index ede6aca..a719279 100644 --- a/attention/src/attention/activate_attention.cpp +++ b/attention/src/attention/activate_attention.cpp @@ -39,7 +39,7 @@ ActivateAttention::ActivateAttention(const std::string & xml_tag_name, const BT: } BT::NodeStatus ActivateAttention::tick() { - RCLCPP_DEBUG(node_->get_logger(), "ACTIVATE_ATTENTION"); + RCLCPP_INFO(node_->get_logger(), "ACTIVATE_ATTENTION"); std_msgs::msg::String msg; msg.data = frame_id_; diff --git a/attention/src/main_attention.cpp b/attention/src/main_attention.cpp index 342190b..5f9a24f 100644 --- a/attention/src/main_attention.cpp +++ b/attention/src/main_attention.cpp @@ -27,7 +27,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared("perception_node"); + 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); @@ -35,7 +35,6 @@ int main(int argc, char * argv[]) 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); @@ -47,7 +46,7 @@ int main(int argc, char * argv[]) factory.registerFromPlugin(loader.getOSName(plugin)); } - std::string pkgpath = ament_index_cpp::get_package_share_directory("perception_bt_nodes"); + 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()); 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/src/perception/get_detection_from_bb.cpp b/perception/src/perception/get_detection_from_bb.cpp new file mode 100644 index 0000000..8144adf --- /dev/null +++ b/perception/src/perception/get_detection_from_bb.cpp @@ -0,0 +1,51 @@ +// 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_from_blackboard.hpp" + +namespace perception +{ + + +GetDetectionFromBB::GetDetectionFromBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + std::string what; + + + getInput("key", key_); + +} +BT::NodeStatus GetDetectionFromBB::tick() +{ + + perception_system_interfaces::msg::Detection detection; + + config().blackboard->get(key_, detection); + rclcpp::spin_some(node_->get_node_base_interface()); + + setOutput("detection", std::make_shared(detection)); + return BT::NodeStatus::SUCCESS; +} + +void GetDetectionFromBB::halt() +{ + perception_pub_->on_deactivate(); +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("GetDetectionFromBB"); +} From c66442536965043a873c3007db66e50e2cdc8e51 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 13 Dec 2024 12:12:18 +0100 Subject: [PATCH 47/60] errors corrected --- perception/CMakeLists.txt | 2 + .../perception/save_detection_in_bb.hpp | 55 +++++++++++++++++++ .../src/perception/get_detection_from_bb.cpp | 8 +-- .../src/perception/save_detection_in_bb.cpp | 43 +++++++++++++++ 4 files changed, 101 insertions(+), 7 deletions(-) create mode 100644 perception/include/perception/save_detection_in_bb.hpp create mode 100644 perception/src/perception/save_detection_in_bb.cpp diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index f529657..494e552 100644 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -55,6 +55,8 @@ set(plugin_sources 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 "") 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..5272845 --- /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/src/perception/get_detection_from_bb.cpp b/perception/src/perception/get_detection_from_bb.cpp index 8144adf..3f1aa99 100644 --- a/perception/src/perception/get_detection_from_bb.cpp +++ b/perception/src/perception/get_detection_from_bb.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception/get_from_blackboard.hpp" +#include "perception/get_detection_from_bb.hpp" namespace perception { @@ -21,11 +21,7 @@ namespace perception GetDetectionFromBB::GetDetectionFromBB(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - std::string what; - - getInput("key", key_); - } BT::NodeStatus GetDetectionFromBB::tick() { @@ -33,7 +29,6 @@ BT::NodeStatus GetDetectionFromBB::tick() perception_system_interfaces::msg::Detection detection; config().blackboard->get(key_, detection); - rclcpp::spin_some(node_->get_node_base_interface()); setOutput("detection", std::make_shared(detection)); return BT::NodeStatus::SUCCESS; @@ -41,7 +36,6 @@ BT::NodeStatus GetDetectionFromBB::tick() void GetDetectionFromBB::halt() { - perception_pub_->on_deactivate(); } } // namespace perception 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..2365b10 --- /dev/null +++ b/perception/src/perception/save_detection_in_bb.cpp @@ -0,0 +1,43 @@ +// 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_)); + return BT::NodeStatus::SUCCESS; +} + +void SaveDetectionInBB::halt() +{ +} + +} // namespace perception + +BT_REGISTER_NODES(factory) { + factory.registerNodeType("SaveDetectionInBB"); +} From 866c75de102d5fa5b64ce3a84f0440d0c906a210 Mon Sep 17 00:00:00 2001 From: roi Date: Fri, 13 Dec 2024 20:38:37 +0100 Subject: [PATCH 48/60] errors corrected --- perception/include/perception/save_detection_in_bb.hpp | 2 +- perception/src/perception/get_detection_from_bb.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/include/perception/save_detection_in_bb.hpp b/perception/include/perception/save_detection_in_bb.hpp index 5272845..db687dc 100644 --- a/perception/include/perception/save_detection_in_bb.hpp +++ b/perception/include/perception/save_detection_in_bb.hpp @@ -41,7 +41,7 @@ class SaveDetectionInBB : public BT::ActionNodeBase return BT::PortsList( { BT::InputPort("key"), - BT::InputPort("detection") + BT::InputPort>("detection") }); } diff --git a/perception/src/perception/get_detection_from_bb.cpp b/perception/src/perception/get_detection_from_bb.cpp index 3f1aa99..2fa40d4 100644 --- a/perception/src/perception/get_detection_from_bb.cpp +++ b/perception/src/perception/get_detection_from_bb.cpp @@ -26,11 +26,11 @@ GetDetectionFromBB::GetDetectionFromBB(const std::string & xml_tag_name, const B BT::NodeStatus GetDetectionFromBB::tick() { - perception_system_interfaces::msg::Detection detection; + std::shared_ptr detection_ptr; - config().blackboard->get(key_, detection); + config().blackboard->get(key_, detection_ptr); - setOutput("detection", std::make_shared(detection)); + setOutput("detection", detection_ptr); return BT::NodeStatus::SUCCESS; } From 71e127f323c362638264f1fb87aa90cb79e72825 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 13 Jan 2025 10:33:59 +0100 Subject: [PATCH 49/60] fixed --- perception/include/perception/is_in_view.hpp | 4 ++-- perception/src/perception/is_in_view.cpp | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/perception/include/perception/is_in_view.hpp b/perception/include/perception/is_in_view.hpp index f6e4199..a912105 100644 --- a/perception/include/perception/is_in_view.hpp +++ b/perception/include/perception/is_in_view.hpp @@ -56,14 +56,14 @@ class IsInView : public BT::ConditionNode { return BT::PortsList( { - BT::InputPort("frame") // Name of the tf frame of the entity to check if is in front + BT::InputPort>("detection") // Detection to check if is in view }); } private: std::shared_ptr node_; - std::string frame_; + std::shared_ptr detection_; tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/perception/src/perception/is_in_view.cpp b/perception/src/perception/is_in_view.cpp index f227e6b..acde617 100644 --- a/perception/src/perception/is_in_view.cpp +++ b/perception/src/perception/is_in_view.cpp @@ -26,18 +26,20 @@ IsInView::IsInView(const std::string & xml_tag_name, const BT::NodeConfiguration { config().blackboard->get("node", node_); - getInput("frame", frame_); + getInput("detection", detection_); } BT::NodeStatus IsInView::tick() { RCLCPP_DEBUG(node_->get_logger(), "IS_IN_VIEW"); rclcpp::spin_some(node_->get_node_base_interface()); - auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); + // auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); pl::getInstance(node_)->update(30); - auto detections = pl::getInstance(node_)->get_detection_at(base2entity_msg); - RCLCPP_DEBUG(node_->get_logger(), "Detection obtained from TF"); + auto detections = pl::getInstance(node_)->get_by_id(detection_->unique_id); + RCLCPP_DEBUG(node_->get_logger(), "Detection obtained by ID"); + // auto detections = pl::getInstance(node_)->get_detection_at(base2entity_msg); + // RCLCPP_DEBUG(node_->get_logger(), "Detection obtained from TF"); if (detections.empty()) { RCLCPP_INFO(node_->get_logger(), "Detection not in view"); From eaf94858e84d5eb6d81d4f8ea51e8e5053f35af9 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 13 Jan 2025 11:51:53 +0100 Subject: [PATCH 50/60] logging --- perception/src/perception/get_detection_from_bb.cpp | 2 +- perception/src/perception/save_detection_in_bb.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/src/perception/get_detection_from_bb.cpp b/perception/src/perception/get_detection_from_bb.cpp index 2fa40d4..17a7509 100644 --- a/perception/src/perception/get_detection_from_bb.cpp +++ b/perception/src/perception/get_detection_from_bb.cpp @@ -29,7 +29,7 @@ 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; } diff --git a/perception/src/perception/save_detection_in_bb.cpp b/perception/src/perception/save_detection_in_bb.cpp index 2365b10..ebcf55a 100644 --- a/perception/src/perception/save_detection_in_bb.cpp +++ b/perception/src/perception/save_detection_in_bb.cpp @@ -29,6 +29,7 @@ SaveDetectionInBB::SaveDetectionInBB(const std::string & xml_tag_name, const BT: 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; } From c4464fcc814738d1520693563bea2075013a4e23 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 13 Jan 2025 11:52:22 +0100 Subject: [PATCH 51/60] provisional till get_by_id works --- perception/src/perception/is_in_view.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/perception/src/perception/is_in_view.cpp b/perception/src/perception/is_in_view.cpp index acde617..b577165 100644 --- a/perception/src/perception/is_in_view.cpp +++ b/perception/src/perception/is_in_view.cpp @@ -33,22 +33,19 @@ BT::NodeStatus IsInView::tick() RCLCPP_DEBUG(node_->get_logger(), "IS_IN_VIEW"); rclcpp::spin_some(node_->get_node_base_interface()); - // auto base2entity_msg = tf_buffer_.lookupTransform("base_link", frame_, tf2::TimePointZero); - pl::getInstance(node_)->update(30); - auto detections = pl::getInstance(node_)->get_by_id(detection_->unique_id); + // 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"); - // auto detections = pl::getInstance(node_)->get_detection_at(base2entity_msg); - // RCLCPP_DEBUG(node_->get_logger(), "Detection obtained from TF"); if (detections.empty()) { - RCLCPP_INFO(node_->get_logger(), "Detection not in view"); + RCLCPP_INFO(node_->get_logger(), "Detection NOT in view"); 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_INFO(node_->get_logger(), "Detection IN view"); return BT::NodeStatus::SUCCESS; } From ce3ac64b56a96ba80960a4445f32d6136b0adf30 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 5 Mar 2025 11:12:20 +0100 Subject: [PATCH 52/60] logging --- perception/src/perception/is_in_view.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/src/perception/is_in_view.cpp b/perception/src/perception/is_in_view.cpp index b577165..d7ca676 100644 --- a/perception/src/perception/is_in_view.cpp +++ b/perception/src/perception/is_in_view.cpp @@ -39,13 +39,13 @@ BT::NodeStatus IsInView::tick() RCLCPP_DEBUG(node_->get_logger(), "Detection obtained by ID"); if (detections.empty()) { - RCLCPP_INFO(node_->get_logger(), "Detection NOT in view"); + 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_INFO(node_->get_logger(), "Detection IN view"); + RCLCPP_DEBUG(node_->get_logger(), "Detection IN view"); return BT::NodeStatus::SUCCESS; } From e83be5e8093d81d45d4c2cce8f20521e9e8d8516 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 5 Mar 2025 11:12:32 +0100 Subject: [PATCH 53/60] logging --- attention/src/attention/activate_attention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/attention/src/attention/activate_attention.cpp b/attention/src/attention/activate_attention.cpp index a719279..ede6aca 100644 --- a/attention/src/attention/activate_attention.cpp +++ b/attention/src/attention/activate_attention.cpp @@ -39,7 +39,7 @@ ActivateAttention::ActivateAttention(const std::string & xml_tag_name, const BT: } BT::NodeStatus ActivateAttention::tick() { - RCLCPP_INFO(node_->get_logger(), "ACTIVATE_ATTENTION"); + RCLCPP_DEBUG(node_->get_logger(), "ACTIVATE_ATTENTION"); std_msgs::msg::String msg; msg.data = frame_id_; From b5647888387837f467d486a40a1be03383dda77e Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 5 Mar 2025 11:12:51 +0100 Subject: [PATCH 54/60] logging --- attention/src/attention/deactivate_attention.cpp | 2 +- hri/src/hri/dialog/speak.cpp | 2 ++ motion/src/motion/navigation/navigate_to.cpp | 16 ++++++++-------- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/attention/src/attention/deactivate_attention.cpp b/attention/src/attention/deactivate_attention.cpp index 8b2672a..4fdd2f9 100644 --- a/attention/src/attention/deactivate_attention.cpp +++ b/attention/src/attention/deactivate_attention.cpp @@ -38,7 +38,7 @@ DeactivateAttention::DeactivateAttention(const std::string & xml_tag_name, const } BT::NodeStatus DeactivateAttention::tick() { - RCLCPP_DEBUG(node_->get_logger(), "ACTIVATE_ATTENTION"); + RCLCPP_DEBUG(node_->get_logger(), "DEACTIVATE_ATTENTION"); std_msgs::msg::String msg; msg.data = ""; diff --git a/hri/src/hri/dialog/speak.cpp b/hri/src/hri/dialog/speak.cpp index e620610..0dd3af6 100644 --- a/hri/src/hri/dialog/speak.cpp +++ b/hri/src/hri/dialog/speak.cpp @@ -32,6 +32,8 @@ Speak::Speak( } 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); diff --git a/motion/src/motion/navigation/navigate_to.cpp b/motion/src/motion/navigation/navigate_to.cpp index 059a7b7..ee5f089 100644 --- a/motion/src/motion/navigation/navigate_to.cpp +++ b/motion/src/motion/navigation/navigate_to.cpp @@ -34,7 +34,7 @@ NavigateTo::NavigateTo( void NavigateTo::on_tick() { - RCLCPP_DEBUG(node_->get_logger(), "NavigateTo ticked"); + 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; @@ -46,7 +46,7 @@ void NavigateTo::on_tick() getInput("distance_tolerance", distance_tolerance); if (tf_frame.length() > 0) { // There is a TF to go, ignore coordinates - RCLCPP_INFO(node_->get_logger(), "Transforming map to %s", tf_frame.c_str()); + 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(); @@ -55,7 +55,7 @@ void NavigateTo::on_tick() // 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(), "Could not transform map to %s: %s", tf_frame.c_str(), ex.what()); + node_->get_logger(), "[NAVIGATE_TO]: could not transform map to %s: %s", tf_frame.c_str(), ex.what()); setStatus(BT::NodeStatus::FAILURE); return; } @@ -73,7 +73,7 @@ void NavigateTo::on_tick() getInput("x", goal.pose.position.x); getInput("y", goal.pose.position.y); - RCLCPP_INFO(node_->get_logger(), "Setting goal to x: %.2f, y: %.2f", goal.pose.position.x, 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; @@ -84,10 +84,10 @@ void NavigateTo::on_tick() goal.header.frame_id = "map"; RCLCPP_INFO( - node_->get_logger(), "Sending coordinates. Frame: %s, x: %f, y: %f", goal.header.frame_id.c_str(),goal.pose.position.x, goal.pose.position.y); + 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(), "Setting distance tolerance to %f", distance_tolerance); + 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; } @@ -97,7 +97,7 @@ void NavigateTo::on_tick() BT::NodeStatus NavigateTo::on_success() { - RCLCPP_INFO(node_->get_logger(), "Navigation succeeded"); + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: navigation succeeded"); if (will_finish_) { return BT::NodeStatus::SUCCESS; } @@ -124,7 +124,7 @@ BT::NodeStatus NavigateTo::on_cancelled() } on_tick(); on_new_goal_received(); - RCLCPP_INFO(node_->get_logger(), "Navigation cancelled"); + RCLCPP_INFO(node_->get_logger(), "[NAVIGATE_TO]: navigation cancelled"); return BT::NodeStatus::RUNNING; } From 6001e38b5da7a2cdb4ee08297e635eb8012a1620 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 5 Mar 2025 11:13:24 +0100 Subject: [PATCH 55/60] update --- perception/include/perception/identify.hpp | 2 ++ perception/src/perception/identify.cpp | 28 +++++++++++++++++++--- 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/perception/include/perception/identify.hpp b/perception/include/perception/identify.hpp index 9a097d0..3f6d86a 100644 --- a/perception/include/perception/identify.hpp +++ b/perception/include/perception/identify.hpp @@ -58,6 +58,8 @@ class Identify : public BT::ActionNodeBase } 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_; diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp index eed1118..983b35e 100644 --- a/perception/src/perception/identify.cpp +++ b/perception/src/perception/identify.cpp @@ -54,10 +54,13 @@ Identify::tick() 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_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(node_->get_logger(), "Perception system did not identify %s", entity_.c_str()); + RCLCPP_WARN_ONCE(node_->get_logger(), "Perception system did not identify %s", entity_.c_str()); return BT::NodeStatus::FAILURE; } @@ -67,12 +70,31 @@ Identify::tick() // }); // Publish the detection - RCLCPP_DEBUG(node_->get_logger(), "Perception system detected %s with confidence %f. Publishing TF", entity_.c_str(), detections[0].score); + 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); 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) { From e39457fb92d470f9b23d34f253476b102e8b06f0 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 5 May 2025 13:17:25 +0200 Subject: [PATCH 56/60] launch --- hri/launch/hri_dependencies.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/hri/launch/hri_dependencies.launch.py b/hri/launch/hri_dependencies.launch.py index 928fdd5..9c8bb34 100644 --- a/hri/launch/hri_dependencies.launch.py +++ b/hri/launch/hri_dependencies.launch.py @@ -52,7 +52,8 @@ def generate_launch_description(): launch_arguments={'stream': 'False', 'language': 'es', 'model_repo': 'ggerganov/whisper.cpp', - 'model_filename': 'ggml-large-v3-turbo-q5_0.bin'} + 'model_filename': 'ggml-large-v3-turbo-q5_0.bin', + 'use_gpu': 'False',} .items() ) @@ -75,7 +76,7 @@ def generate_launch_description(): {'frame_id': ''}, {'model': tts_model}, {'speaker_wav': ''}, - {'device': 'cuda'}] + {'device': 'cpu'}] ) ld = LaunchDescription() From fc19119599bf995a5ec4df7ee1eeeabffefbb632 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 6 May 2025 13:24:21 +0200 Subject: [PATCH 57/60] minor --- hri/thirdparty.repos | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hri/thirdparty.repos b/hri/thirdparty.repos index 9c190f3..f543fc6 100644 --- a/hri/thirdparty.repos +++ b/hri/thirdparty.repos @@ -15,5 +15,7 @@ repositories: type: git url: https://github.com/mgonzs13/tts_ros.git version: main - - \ No newline at end of file + thirdparty/llama_ros: + type: git + url: https://github.com/mgonzs13/llama_ros.git + version: main \ No newline at end of file From 967511745bd166928d65487a67197bea152f0448 Mon Sep 17 00:00:00 2001 From: roi Date: Tue, 6 May 2025 16:49:20 +0200 Subject: [PATCH 58/60] bug --- hri/src/hri/dialog/query.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hri/src/hri/dialog/query.cpp b/hri/src/hri/dialog/query.cpp index 117d996..50b5877 100644 --- a/hri/src/hri/dialog/query.cpp +++ b/hri/src/hri/dialog/query.cpp @@ -40,7 +40,7 @@ Query::Query( node_->get_parameter("placeholder", placeholder_); try { - std::string pkg_dir = ament_index_cpp::get_package_share_directory("hri"); + 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) { From d00d55253963c568af9745443c01baccfb4edefe Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 21 May 2025 13:55:53 +0200 Subject: [PATCH 59/60] tp --- hri/thirdparty.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hri/thirdparty.repos b/hri/thirdparty.repos index f543fc6..832b973 100644 --- a/hri/thirdparty.repos +++ b/hri/thirdparty.repos @@ -1,6 +1,6 @@ repositories: thirdparty/cascade_lifecycle: - type: git + type: jazzy-devel url: https://github.com/fmrico/cascade_lifecycle version: main thirdparty/whisper_ros: From 3bcd69abfbfce9da88a2ddb872974d166ac36821 Mon Sep 17 00:00:00 2001 From: roi Date: Wed, 21 May 2025 14:07:44 +0200 Subject: [PATCH 60/60] minor --- motion/src/motion/base/spin.cpp | 4 ++-- perception/src/perception/identify.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/motion/src/motion/base/spin.cpp b/motion/src/motion/base/spin.cpp index 452b26b..a4c21cc 100644 --- a/motion/src/motion/base/spin.cpp +++ b/motion/src/motion/base/spin.cpp @@ -36,10 +36,10 @@ BT::NodeStatus Spin::tick() // getInput("direction", direction_); if (forever_) { - RCLCPP_DEBUG(node_->get_logger(), "Spinning 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_DEBUG(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s", angle_, speed_); + RCLCPP_INFO_ONCE(node_->get_logger(), "Spinning %.2f degrees at %.2f rad/s", angle_, speed_); } if (status() == BT::NodeStatus::IDLE) { diff --git a/perception/src/perception/identify.cpp b/perception/src/perception/identify.cpp index 983b35e..5e0f181 100644 --- a/perception/src/perception/identify.cpp +++ b/perception/src/perception/identify.cpp @@ -35,7 +35,7 @@ Identify::Identify( if (!getInput("confidence", confidence_)) { RCLCPP_WARN(node_->get_logger(), "No confidence provided. Using default value %f", confidence_); } else { - RCLCPP_INFO(node_->get_logger(), "Confidence threshold: %.2f", confidence_); + RCLCPP_DEBUG(node_->get_logger(), "Confidence threshold: %.2f", confidence_); } } @@ -48,7 +48,7 @@ Identify::tick() std::vector detections; if (!getInput("detection", detection_)) { - RCLCPP_ERROR(node_->get_logger(), "No detection at input"); + RCLCPP_ERROR_ONCE(node_->get_logger(), "[IDENTIFY]: No detection at input"); return BT::NodeStatus::FAILURE; } @@ -60,7 +60,7 @@ Identify::tick() // save_detection_tf_to_bb(detection_, "map", entity_); if (detections.empty()) { - RCLCPP_WARN_ONCE(node_->get_logger(), "Perception system did not identify %s", entity_.c_str()); + RCLCPP_WARN_ONCE(node_->get_logger(), "[IDENTIFY]: Perception system did not identify %s", entity_.c_str()); return BT::NodeStatus::FAILURE; } @@ -72,6 +72,7 @@ Identify::tick() // 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; }