From 756180010f2cb8fbda5d51aeb7787461ffdcd088 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 15 Feb 2024 12:15:05 +0000 Subject: [PATCH 01/35] added behaviortree_ros2 to the repository because it is not in rosped Signed-off-by: Jakub Delicat --- panther_gazebo/package.xml | 2 +- panther_manager/CMakeLists.txt | 26 + .../behaviortree_ros2/bt_action_node.hpp | 454 ++++++++++++++++++ .../behaviortree_ros2/bt_service_node.hpp | 338 +++++++++++++ .../behaviortree_ros2/bt_topic_pub_node.hpp | 192 ++++++++ .../behaviortree_ros2/bt_topic_sub_node.hpp | 303 ++++++++++++ .../include/behaviortree_ros2/plugins.hpp | 68 +++ .../behaviortree_ros2/ros_node_params.hpp | 45 ++ panther_manager/package.xml | 28 ++ panther_manager/src/bt_ros2.cpp | 4 + 10 files changed, 1459 insertions(+), 1 deletion(-) create mode 100644 panther_manager/CMakeLists.txt create mode 100644 panther_manager/include/behaviortree_ros2/bt_action_node.hpp create mode 100644 panther_manager/include/behaviortree_ros2/bt_service_node.hpp create mode 100644 panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp create mode 100644 panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp create mode 100644 panther_manager/include/behaviortree_ros2/plugins.hpp create mode 100644 panther_manager/include/behaviortree_ros2/ros_node_params.hpp create mode 100644 panther_manager/package.xml create mode 100644 panther_manager/src/bt_ros2.cpp diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index afdb95efd..7b88a3522 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -3,7 +3,7 @@ panther_gazebo 0.0.1 - The panther_description package + The panther_gazebo package Husarion Apache License 2.0 diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt new file mode 100644 index 000000000..529f66696 --- /dev/null +++ b/panther_manager/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(panther_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(PACKAGE_INCLUDE_DEPENDS + ament_cmake + rclcpp + rclcpp_action + ament_index_cpp + behaviortree_cpp) + +foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) +add_library(behaviortree_ros2 src/bt_ros2.cpp) +ament_target_dependencies(behaviortree_ros2 ${PACKAGE_INCLUDE_DEPENDS}) + +ament_export_include_directories(include) +ament_export_dependencies(${PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/panther_manager/include/behaviortree_ros2/bt_action_node.hpp b/panther_manager/include/behaviortree_ros2/bt_action_node.hpp new file mode 100644 index 000000000..8e4d49f8f --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/bt_action_node.hpp @@ -0,0 +1,454 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2023 Davide Faconti +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "behaviortree_ros2/ros_node_params.hpp" + +namespace BT +{ + +enum ActionNodeErrorCode +{ + SERVER_UNREACHABLE, + SEND_GOAL_TIMEOUT, + GOAL_REJECTED_BY_SERVER, + ACTION_ABORTED, + ACTION_CANCELLED, + INVALID_GOAL +}; + +inline const char* toStr(const ActionNodeErrorCode& err) +{ + switch (err) + { + case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; + case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; + case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; + case ACTION_ABORTED: return "ACTION_ABORTED"; + case ACTION_CANCELLED: return "ACTION_CANCELLED"; + case INVALID_GOAL: return "INVALID_GOAL"; + } + return nullptr; +} + +/** + * @brief Abstract class to wrap rclcpp_action::Client<> + * + * For instance, given the type AddTwoInts described in this tutorial: + * https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html + * + * the corresponding wrapper would be: + * + * class FibonacciNode: public RosActionNode + * + * RosActionNode will try to be non-blocking for the entire duration of the call. + * The derived class must reimplement the virtual methods as described below. + * + * The name of the action will be determined as follows: + * + * 1. If a value is passes in the InputPort "action_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosActionNode : public BT::ActionNodeBase +{ + +public: + // Type definitions + using ActionType = ActionT; + using ActionClient = typename rclcpp_action::Client; + using ActionClientPtr = std::shared_ptr; + using Goal = typename ActionT::Goal; + using GoalHandle = typename rclcpp_action::ClientGoalHandle; + using WrappedResult = typename rclcpp_action::ClientGoalHandle::WrappedResult; + using Feedback = typename ActionT::Feedback; + + /** To register this class into the factory, use: + * + * factory.registerNodeType<>(node_name, params); + * + */ + explicit RosActionNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosActionNode() = default; + + /** + * @brief Any subclass of RosActionNode that has ports must implement a + * providedPorts method and call providedBasicPorts in it. + * + * @param addition Additional ports to add to BT port list + * @return PortsList containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("action_name", "__default__placeholder__", "Action server name") + }; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + /// @brief Callback executed when the node is halted. Note that cancelGoal() + /// is done automatically. + virtual void onHalt() {} + + /** setGoal s a callback that allows the user to set + * the goal message (ActionT::Goal). + * + * @param goal the goal to be sent to the action server. + * + * @return false if the request should not be sent. In that case, + * RosActionNode::onFailure(INVALID_GOAL) will be called. + */ + virtual bool setGoal(Goal& goal) = 0; + + /** Callback invoked when the result is received by the server. + * It is up to the user to define if the action returns SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onResultReceived(const WrappedResult& result) = 0; + + /** Callback invoked when the feedback is received. + * It generally returns RUNNING, but the user can also use this callback to cancel the + * current action and return SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onFeedback(const std::shared_ptr /*feedback*/) + { + return NodeStatus::RUNNING; + } + + /** Callback invoked when something goes wrong. + * It must return either SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) + { + return NodeStatus::FAILURE; + } + + /// Method used to send a request to the Action server to cancel the current goal + void cancelGoal(); + + + /// The default halt() implementation will call cancelGoal if necessary. + void halt() override final; + + NodeStatus tick() override final; + +protected: + + std::shared_ptr node_; + std::string prev_action_name_; + bool action_name_may_change_ = false; + const std::chrono::milliseconds server_timeout_; + const std::chrono::milliseconds wait_for_server_timeout_; + +private: + + ActionClientPtr action_client_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::shared_future future_goal_handle_; + typename GoalHandle::SharedPtr goal_handle_; + + rclcpp::Time time_goal_sent_; + NodeStatus on_feedback_state_change_; + bool goal_received_; + WrappedResult result_; + + bool createClient(const std::string &action_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosActionNode::RosActionNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams ¶ms): + BT::ActionNodeBase(instance_name, conf), + node_(params.nh), + server_timeout_(params.server_timeout), + wait_for_server_timeout_(params.wait_for_server_timeout) +{ + // Three cases: + // - we use the default action_name in RosNodeParams when port is empty + // - we use the action_name in the port and it is a static string. + // - we use the action_name in the port and it is blackboard entry. + + // check port remapping + auto portIt = config().input_ports.find("action_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_action_name = portIt->second; + + if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [action_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_action_name)) + { + // If the content of the port "action_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createClient(bb_action_name); + } + else { + action_name_may_change_ = true; + // createClient will be invoked in the first tick(). + } + } + else { + + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [action_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } +} + +template inline + bool RosActionNode::createClient(const std::string& action_name) +{ + if(action_name.empty()) + { + throw RuntimeError("action_name is empty"); + } + + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); + + prev_action_name_ = action_name; + + bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); + if(!found) + { + RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", + name().c_str(), prev_action_name_.c_str()); + } + return found; +} + +template inline + NodeStatus RosActionNode::tick() +{ + // First, check if the action_client_ is valid and that the name of the + // action_name in the port didn't change. + // otherwise, create a new client + if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + { + std::string action_name; + getInput("action_name", action_name); + if(prev_action_name_ != action_name) + { + createClient(action_name); + } + } + + //------------------------------------------ + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosActionNode: the callback must return either SUCCESS of FAILURE"); + } + return status; + }; + + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) + { + setStatus(NodeStatus::RUNNING); + + goal_received_ = false; + future_goal_handle_ = {}; + on_feedback_state_change_ = NodeStatus::RUNNING; + result_ = {}; + + Goal goal; + + if( !setGoal(goal) ) + { + return CheckStatus( onFailure(INVALID_GOAL) ); + } + + typename ActionClient::SendGoalOptions goal_options; + + //-------------------- + goal_options.feedback_callback = + [this](typename GoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + on_feedback_state_change_ = onFeedback(feedback); + if( on_feedback_state_change_ == NodeStatus::IDLE) + { + throw std::logic_error("onFeedback must not return IDLE"); + } + emitWakeUpSignal(); + }; + //-------------------- + goal_options.result_callback = + [this](const WrappedResult& result) + { + if (goal_handle_->get_goal_id() == result.goal_id) { + RCLCPP_DEBUG( node_->get_logger(), "result_callback" ); + result_ = result; + emitWakeUpSignal(); + } + }; + //-------------------- + goal_options.goal_response_callback = + [this](typename GoalHandle::SharedPtr const future_handle) + { + auto goal_handle_ = future_handle.get(); + if (!goal_handle_) + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + //-------------------- + + future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); + time_goal_sent_ = node_->now(); + + return NodeStatus::RUNNING; + } + + if (status() == NodeStatus::RUNNING) + { + callback_group_executor_.spin_some(); + + // FIRST case: check if the goal request has a timeout + if( !goal_received_ ) + { + auto nodelay = std::chrono::milliseconds(0); + auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); + + auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); + if (ret != rclcpp::FutureReturnCode::SUCCESS) + { + if( (node_->now() - time_goal_sent_) > timeout ) + { + return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); + } + else{ + return NodeStatus::RUNNING; + } + } + else + { + goal_received_ = true; + goal_handle_ = future_goal_handle_.get(); + future_goal_handle_ = {}; + + if (!goal_handle_) { + return CheckStatus( onFailure( GOAL_REJECTED_BY_SERVER ) ); + } + } + } + + // SECOND case: onFeedback requested a stop + if( on_feedback_state_change_ != NodeStatus::RUNNING ) + { + cancelGoal(); + return on_feedback_state_change_; + } + // THIRD case: result received, requested a stop + if( result_.code != rclcpp_action::ResultCode::UNKNOWN) + { + if( result_.code == rclcpp_action::ResultCode::ABORTED ) + { + return CheckStatus( onFailure( ACTION_ABORTED ) ); + } + else if( result_.code == rclcpp_action::ResultCode::CANCELED ) + { + return CheckStatus( onFailure( ACTION_CANCELLED ) ); + } + else{ + return CheckStatus( onResultReceived( result_ ) ); + } + } + } + return NodeStatus::RUNNING; +} + +template inline + void RosActionNode::halt() +{ + if(status() == BT::NodeStatus::RUNNING) + { + cancelGoal(); + onHalt(); + } +} + +template inline + void RosActionNode::cancelGoal() +{ + auto future_result = action_client_->async_get_result(goal_handle_); + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + + if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( node_->get_logger(), "Failed to cancel action server for [%s]", + prev_action_name_.c_str()); + } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", + prev_action_name_.c_str()); + } +} + + + + +} // namespace BT + diff --git a/panther_manager/include/behaviortree_ros2/bt_service_node.hpp b/panther_manager/include/behaviortree_ros2/bt_service_node.hpp new file mode 100644 index 000000000..66477e3b2 --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/bt_service_node.hpp @@ -0,0 +1,338 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 Davide Faconti +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/ros_node_params.hpp" + +namespace BT +{ + +enum ServiceNodeErrorCode +{ + SERVICE_UNREACHABLE, + SERVICE_TIMEOUT, + INVALID_REQUEST, + SERVICE_ABORTED +}; + +inline const char* toStr(const ServiceNodeErrorCode& err) +{ + switch (err) + { + case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; + case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; + case INVALID_REQUEST: return "INVALID_REQUEST"; + case SERVICE_ABORTED: return "SERVICE_ABORTED"; + } + return nullptr; +} + +/** + * @brief Abstract class use to wrap rclcpp::Client<> + * + * For instance, given the type AddTwoInts described in this tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html + * + * the corresponding wrapper would be: + * + * class AddTwoNumbers: public RosServiceNode + * + * RosServiceNode will try to be non-blocking for the entire duration of the call. + * The derived class must reimplement the virtual methods as described below. + * + * The name of the service will be determined as follows: + * + * 1. If a value is passes in the InputPort "service_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosServiceNode : public BT::ActionNodeBase +{ + +public: + // Type definitions + using ServiceClient = typename rclcpp::Client; + using Request = typename ServiceT::Request; + using Response = typename ServiceT::Response; + + /** To register this class into the factory, use: + * + * factory.registerNodeType<>(node_name, params); + */ + explicit RosServiceNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const BT::RosNodeParams& params); + + virtual ~RosServiceNode() = default; + + /** + * @brief Any subclass of RosServiceNode that has ports must implement a + * providedPorts method and call providedBasicPorts in it. + * + * @param addition Additional ports to add to BT port list + * @return PortsList containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("service_name", "__default__placeholder__", "Service name") + }; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + NodeStatus tick() override final; + + /// The default halt() implementation. + void halt() override; + + /** setRequest is a callback that allows the user to set + * the request message (ServiceT::Request). + * + * @param request the request to be sent to the service provider. + * + * @return false if the request should not be sent. In that case, + * RosServiceNode::onFailure(INVALID_REQUEST) will be called. + */ + virtual bool setRequest(typename Request::SharedPtr& request) = 0; + + /** Callback invoked when the response is received by the server. + * It is up to the user to define if this returns SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) = 0; + + /** Callback invoked when something goes wrong; you can override it. + * It must return either SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) + { + return NodeStatus::FAILURE; + } + +protected: + + std::shared_ptr node_; + std::string prev_service_name_; + bool service_name_may_change_ = false; + const std::chrono::milliseconds service_timeout_; + const std::chrono::milliseconds wait_for_service_timeout_; + +private: + + typename std::shared_ptr service_client_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::shared_future future_response_; + + rclcpp::Time time_request_sent_; + NodeStatus on_feedback_state_change_; + bool response_received_; + typename Response::SharedPtr response_; + + bool createClient(const std::string &service_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosServiceNode::RosServiceNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params): + BT::ActionNodeBase(instance_name, conf), + node_(params.nh), + service_timeout_(params.server_timeout), + wait_for_service_timeout_(params.wait_for_server_timeout) +{ + // check port remapping + auto portIt = config().input_ports.find("service_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_service_name = portIt->second; + + if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [service_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_service_name)) + { + // If the content of the port "service_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createClient(bb_service_name); + } + else { + service_name_may_change_ = true; + // createClient will be invoked in the first tick(). + } + } + else { + + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [service_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } +} + +template inline + bool RosServiceNode::createClient(const std::string& service_name) +{ + if(service_name.empty()) + { + throw RuntimeError("service_name is empty"); + } + + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); + prev_service_name_ = service_name; + + bool found = service_client_->wait_for_service(wait_for_service_timeout_); + if(!found) + { + RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", + name().c_str(), prev_service_name_.c_str()); + } + return found; +} + +template inline + NodeStatus RosServiceNode::tick() +{ + // First, check if the service_client_ is valid and that the name of the + // service_name in the port didn't change. + // otherwise, create a new client + if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + { + std::string service_name; + getInput("service_name", service_name); + if(prev_service_name_ != service_name) + { + createClient(service_name); + } + } + + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or FAILURE"); + } + return status; + }; + + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) + { + setStatus(NodeStatus::RUNNING); + + response_received_ = false; + future_response_ = {}; + on_feedback_state_change_ = NodeStatus::RUNNING; + response_ = {}; + + typename Request::SharedPtr request = std::make_shared(); + + if( !setRequest(request) ) + { + return CheckStatus( onFailure(INVALID_REQUEST) ); + } + + future_response_ = service_client_->async_send_request(request).share(); + time_request_sent_ = node_->now(); + + return NodeStatus::RUNNING; + } + + if (status() == NodeStatus::RUNNING) + { + callback_group_executor_.spin_some(); + + // FIRST case: check if the goal request has a timeout + if( !response_received_ ) + { + auto const nodelay = std::chrono::milliseconds(0); + auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); + + auto ret = callback_group_executor_.spin_until_future_complete(future_response_, nodelay); + + if (ret != rclcpp::FutureReturnCode::SUCCESS) + { + if( (node_->now() - time_request_sent_) > timeout ) + { + return CheckStatus( onFailure(SERVICE_TIMEOUT) ); + } + else{ + return NodeStatus::RUNNING; + } + } + else + { + response_received_ = true; + response_ = future_response_.get(); + future_response_ = {}; + + if (!response_) { + throw std::runtime_error("Request was rejected by the service"); + } + } + } + + // SECOND case: response received + return CheckStatus( onResponseReceived( response_ ) ); + } + return NodeStatus::RUNNING; +} + +template inline + void RosServiceNode::halt() +{ + if( status() == NodeStatus::RUNNING ) + { + resetStatus(); + } +} + + +} // namespace BT + diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp new file mode 100644 index 000000000..ebeb1f5b0 --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -0,0 +1,192 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_ros2/ros_node_params.hpp" + +namespace BT +{ + +/** + * @brief Abstract class to wrap a ROS publisher + * + */ +template +class RosTopicPubNode : public BT::ConditionNode +{ + +public: + // Type definitions + using Publisher = typename rclcpp::Publisher; + + /** You are not supposed to instantiate this class directly, the factory will do it. + * To register this class into the factory, use: + * + * RegisterRosAction(factory, params) + * + * Note that if the external_action_client is not set, the constructor will build its own. + * */ + explicit RosTopicPubNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosTopicPubNode() = default; + + /** + * @brief Any subclass of RosTopicPubNode that has additinal ports must provide a + * providedPorts method and call providedBasicPorts in it. + * + * @param addition Additional ports to add to BT port list + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("topic_name", "__default__placeholder__", "Topic name") + }; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + NodeStatus tick() override final; + + /** + * @brief setMessage is a callback invoked in tick to allow the user to pass + * the message to be published. + * + * @param msg the message. + * @return return false if anything is wrong and we must not send the message. + * the Condition will return FAILURE. + */ + virtual bool setMessage(TopicT& msg) = 0; + +protected: + + std::shared_ptr node_; + std::string prev_topic_name_; + bool topic_name_may_change_ = false; + +private: + + std::shared_ptr publisher_; + + bool createPublisher(const std::string& topic_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosTopicPubNode::RosTopicPubNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), + node_(params.nh) +{ + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_topic_name = portIt->second; + + if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createPublisher(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createPublisher(bb_topic_name); + } + else { + topic_name_may_change_ = true; + // createPublisher will be invoked in the first tick(). + } + } + else { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createPublisher(params.default_port_value); + } + } +} + +template inline + bool RosTopicPubNode::createPublisher(const std::string& topic_name) +{ + if(topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + + publisher_ = node_->create_publisher(topic_name, 1); + prev_topic_name_ = topic_name; + return true; +} + +template inline + NodeStatus RosTopicPubNode::tick() +{ + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + if(!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) + { + std::string topic_name; + getInput("topic_name", topic_name); + if(prev_topic_name_ != topic_name) + { + createPublisher(topic_name); + } + } + + T msg; + if (!setMessage(msg)) + { + return NodeStatus::FAILURE; + } + publisher_->publish(msg); + return NodeStatus::SUCCESS; +} + +} // namespace BT + diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp new file mode 100644 index 000000000..311594b67 --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -0,0 +1,303 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/ros_node_params.hpp" +#include + +namespace BT +{ + + +/** + * @brief Abstract class to wrap a Topic subscriber. + * Considering the example in the tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html + * + * The corresponding wrapper would be: + * + * class SubscriberNode: RosTopicSubNode + * + * The name of the topic will be determined as follows: + * + * 1. If a value is passes in the InputPort "topic_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosTopicSubNode : public BT::ConditionNode +{ + public: + // Type definitions + using Subscriber = typename rclcpp::Subscription; + + protected: + struct SubscriberInstance + { + void init(std::shared_ptr node, const std::string& topic_name) + { + // create a callback group for this particular instance + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group( + callback_group, node->get_node_base_interface()); + + rclcpp::SubscriptionOptions option; + option.callback_group = callback_group; + + // The callback will broadcast to all the instances of RosTopicSubNode + auto callback = [this](const std::shared_ptr msg) + { + broadcaster(msg); + }; + subscriber = node->create_subscription(topic_name, 1, callback, option); + } + + std::shared_ptr subscriber; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + boost::signals2::signal)> broadcaster; + + + }; + + static std::mutex& registryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> subscribers_registry; + return subscribers_registry; + } + + std::shared_ptr node_; + std::shared_ptr sub_instance_ = nullptr; + std::shared_ptr last_msg_; + std::string topic_name_; + boost::signals2::connection signal_connection_; + std::string subscriber_key_; + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + public: + + /** You are not supposed to instantiate this class directly, the factory will do it. + * To register this class into the factory, use: + * + * RegisterRosAction(factory, params) + * + * Note that if the external_action_client is not set, the constructor will build its own. + * */ + explicit RosTopicSubNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosTopicSubNode() + { + signal_connection_.disconnect(); + // remove the subscribers from the static registry when the ALL the + // instances of RosTopicSubNode are destroyed (i.e., the tree is destroyed) + if(sub_instance_) + { + sub_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); + auto it = registry.find(subscriber_key_); + // when the reference count is 1, means that the only instance is owned by the + // registry itself. Delete it + if(it != registry.end() && it->second.use_count() <= 1) + { + registry.erase(it); + RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); + } + } + } + + /** + * @brief Any subclass of RosTopicNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("topic_name", "__default__placeholder__", "Topic name") + }; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + NodeStatus tick() override final; + + /** Callback invoked in the tick. You must return either SUCCESS of FAILURE + * + * @param last_msg the latest message received, since the last tick. + * Will be empty if no new message received. + * + * @return the new status of the Node, based on last_msg + */ + virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + +private: + + bool createSubscriber(const std::string& topic_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), + node_(params.nh) +{ + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_topic_name = portIt->second; + + if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createSubscriber(bb_topic_name); + } + else { + // do nothing + // createSubscriber will be invoked in the first tick(). + } + } + else { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } +} + +template inline + bool RosTopicSubNode::createSubscriber(const std::string& topic_name) +{ + if(topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + if(sub_instance_) + { + throw RuntimeError("Can't call createSubscriber more than once"); + } + + // find SubscriberInstance in the registry + std::unique_lock lk(registryMutex()); + subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + auto& registry = getRegistry(); + auto it = registry.find(subscriber_key_); + if(it == registry.end()) + { + it = registry.insert( {subscriber_key_, std::make_shared() }).first; + sub_instance_ = it->second; + sub_instance_->init(node_, topic_name); + + RCLCPP_INFO(logger(), + "Node [%s] created Subscriber to topic [%s]", + name().c_str(), topic_name.c_str() ); + } + else { + sub_instance_ = it->second; + } + + + // add "this" as received of the broadcaster + signal_connection_ = sub_instance_->broadcaster.connect( + [this](const std::shared_ptr msg) { last_msg_ = msg; } ); + + topic_name_ = topic_name; + return true; +} + + +template inline + NodeStatus RosTopicSubNode::tick() +{ + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + if(!sub_instance_) + { + std::string topic_name; + getInput("topic_name", topic_name); + createSubscriber(topic_name); + } + + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosTopicSubNode: the callback must return" + "either SUCCESS or FAILURE"); + } + return status; + }; + sub_instance_->callback_group_executor.spin_some(); + auto status = CheckStatus (onTick(last_msg_)); + last_msg_ = nullptr; + + return status; +} + +} // namespace BT + diff --git a/panther_manager/include/behaviortree_ros2/plugins.hpp b/panther_manager/include/behaviortree_ros2/plugins.hpp new file mode 100644 index 000000000..d94f52933 --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/plugins.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2023 Davide Faconti +// +// 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. + +#pragma once + +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_ros2/ros_node_params.hpp" + + +// Use this macro to generate a plugin for: +// +// - BT::RosActionNode +// - BT::RosServiceNode +// - BT::RosTopicPubNode +// - BT::RosTopicSubNode +// +// - First argument: type to register (name of the class) +// - Second argument: string with the registration name +// +// Usage example: +// CreateRosNodePlugin(MyClassName, "MyClassName"); + +#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ +BTCPP_EXPORT void \ +BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ + const BT::RosNodeParams& params) \ +{ \ + factory.registerNodeType(REGISTRATION_NAME, params); \ +} \ + + +/** + * @brief RegisterRosNode function used to load a plugin and register + * the containing Node definition. + * + * @param factory the factory where the node should be registered. + * @param filepath path to the plugin. + * @param params parameters to pass to the instances of the Node. + */ +inline +void RegisterRosNode(BT::BehaviorTreeFactory& factory, + const std::filesystem::path& filepath, + const BT::RosNodeParams& params) +{ + BT::SharedLibrary loader(filepath.generic_string()); + typedef void (*Func)(BT::BehaviorTreeFactory&, + const BT::RosNodeParams&); + auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + func(factory, params); +} + + + + diff --git a/panther_manager/include/behaviortree_ros2/ros_node_params.hpp b/panther_manager/include/behaviortree_ros2/ros_node_params.hpp new file mode 100644 index 000000000..148c086d3 --- /dev/null +++ b/panther_manager/include/behaviortree_ros2/ros_node_params.hpp @@ -0,0 +1,45 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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. + +#pragma once + +#include +#include +#include +#include + +namespace BT +{ + +struct RosNodeParams +{ + std::shared_ptr nh; + + // This has different meaning based on the context: + // + // - RosActionNode: name of the action server + // - RosServiceNode: name of the service + // - RosTopicPubNode: name of the topic to publish to + // - RosTopicSubNode: name of the topic to subscribe to + std::string default_port_value; + + // parameters used only by service client and action clients + + // timeout when sending a request + std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); + // timeout used when detecting the server the first time + std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); +}; + +} diff --git a/panther_manager/package.xml b/panther_manager/package.xml new file mode 100644 index 000000000..30e3083eb --- /dev/null +++ b/panther_manager/package.xml @@ -0,0 +1,28 @@ + + + + panther_manager + + 0.0.1 + The panther_manager package + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/panther_ros + https://github.com/husarion/panther_ros/issues + + Jakub Delicat + + libboost-dev + rclcpp + rclcpp_action + behaviortree_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/panther_manager/src/bt_ros2.cpp b/panther_manager/src/bt_ros2.cpp new file mode 100644 index 000000000..c4769ade7 --- /dev/null +++ b/panther_manager/src/bt_ros2.cpp @@ -0,0 +1,4 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "behaviortree_ros2/bt_topic_sub_node.hpp" +#include "behaviortree_ros2/bt_topic_pub_node.hpp" From 067feb5ffb525305cdbbad6ceb35d75b8d91b68c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 15 Feb 2024 12:49:59 +0000 Subject: [PATCH 02/35] added set bool service Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 17 ++++++++++++++++- panther_manager/package.xml | 1 + 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 529f66696..d7932b2ef 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -10,7 +10,9 @@ set(PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_action ament_index_cpp - behaviortree_cpp) + behaviortree_cpp + std_srvs +) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -18,6 +20,19 @@ endforeach() include_directories(include) add_library(behaviortree_ros2 src/bt_ros2.cpp) + +add_library(call_set_bool_service_bt_node SHARED src/plugins/call_set_bool_service_node.cpp) +list(APPEND plugin_libs call_set_bool_service_bt_node) + +foreach(bt_plugin ${plugin_libs}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) + ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) +endforeach() + +install(TARGETS + DESTINATION lib/${PROJECT_NAME} +) + ament_target_dependencies(behaviortree_ros2 ${PACKAGE_INCLUDE_DEPENDS}) ament_export_include_directories(include) diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 30e3083eb..d62b4345d 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -18,6 +18,7 @@ rclcpp rclcpp_action behaviortree_cpp + std_srvs ament_lint_auto ament_lint_common From 411d3289d59786f457861cb3eb97a153ad8fd8e1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 15 Feb 2024 12:50:56 +0000 Subject: [PATCH 03/35] added set bool service Signed-off-by: Jakub Delicat --- .../plugins/call_set_bool_service_node.hpp | 30 +++++++++++++++++++ .../plugins/call_set_bool_service_node.cpp | 27 +++++++++++++++++ 2 files changed, 57 insertions(+) create mode 100644 panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp create mode 100644 panther_manager/src/plugins/call_set_bool_service_node.cpp diff --git a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp new file mode 100644 index 000000000..f40f649d4 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp @@ -0,0 +1,30 @@ +#ifndef PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ + +#include +#include +#include + +namespace panther_manager +{ + +class CallSetBoolService : public BT::RosServiceNode +{ +public: + CallSetBoolService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ BT::InputPort("data", "true / false value") }); + } + + virtual bool setRequest(typename Request::SharedPtr& request) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ diff --git a/panther_manager/src/plugins/call_set_bool_service_node.cpp b/panther_manager/src/plugins/call_set_bool_service_node.cpp new file mode 100644 index 000000000..a56dcb127 --- /dev/null +++ b/panther_manager/src/plugins/call_set_bool_service_node.cpp @@ -0,0 +1,27 @@ +#include +namespace panther_manager +{ + +bool CallSetBoolService::setRequest(typename Request::SharedPtr& request) +{ + if (!getInput("data", request->data)) + { + throw BT::RuntimeError("[", name(), "] Failed to get input [data]"); + } + return request->data; +} + +BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::SharedPtr& response) +{ + if (!response->success) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to call " << prev_service_name_ << "service, message: " << response->message); + return BT::NodeStatus::FAILURE; + } + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Successfully called "<< prev_service_name_ << " service, message: " << response->message); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallSetBoolService, "CallSetBoolService"); From da67b2e065cc020268e33556af7e2c646c229eb6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 15 Feb 2024 13:21:41 +0000 Subject: [PATCH 04/35] Added trigger service call plugin Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 3 ++ .../plugins/call_set_bool_service_node.hpp | 1 + .../plugins/call_trigger_service_node.hpp | 30 +++++++++++++++++++ .../plugins/call_set_bool_service_node.cpp | 19 ++++++++---- .../src/plugins/call_trigger_service_node.cpp | 26 ++++++++++++++++ 5 files changed, 74 insertions(+), 5 deletions(-) create mode 100644 panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp create mode 100644 panther_manager/src/plugins/call_trigger_service_node.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index d7932b2ef..47f595ed9 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -24,6 +24,9 @@ add_library(behaviortree_ros2 src/bt_ros2.cpp) add_library(call_set_bool_service_bt_node SHARED src/plugins/call_set_bool_service_node.cpp) list(APPEND plugin_libs call_set_bool_service_bt_node) +add_library(call_trigger_service_bt_node SHARED src/plugins/call_trigger_service_node.cpp) +list(APPEND plugin_libs call_trigger_service_bt_node) + foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp index f40f649d4..fee652eee 100644 --- a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp @@ -23,6 +23,7 @@ class CallSetBoolService : public BT::RosServiceNode virtual bool setRequest(typename Request::SharedPtr& request) override; virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; + virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode /*error*/) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp b/panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp new file mode 100644 index 000000000..6c6a77a85 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp @@ -0,0 +1,30 @@ +#ifndef PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ + +#include +#include +#include + +namespace panther_manager +{ + +class CallTriggerService : public BT::RosServiceNode +{ +public: + CallTriggerService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({}); + } + + virtual bool setRequest(typename Request::SharedPtr& /* request*/ ) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ diff --git a/panther_manager/src/plugins/call_set_bool_service_node.cpp b/panther_manager/src/plugins/call_set_bool_service_node.cpp index a56dcb127..411c10f4e 100644 --- a/panther_manager/src/plugins/call_set_bool_service_node.cpp +++ b/panther_manager/src/plugins/call_set_bool_service_node.cpp @@ -6,21 +6,30 @@ bool CallSetBoolService::setRequest(typename Request::SharedPtr& request) { if (!getInput("data", request->data)) { - throw BT::RuntimeError("[", name(), "] Failed to get input [data]"); + return false; } - return request->data; + return true; } BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::SharedPtr& response) { - if (!response->success) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to call " << prev_service_name_ << "service, message: " << response->message); + if (!response->success) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG_STREAM(node_->get_logger(), "Successfully called "<< prev_service_name_ << " service, message: " << response->message); + RCLCPP_DEBUG_STREAM(node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CallSetBoolService::onFailure(BT::ServiceNodeErrorCode /*error*/) +{ + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [data]"); + return BT::NodeStatus::FAILURE; +} + } // namespace panther_manager #include diff --git a/panther_manager/src/plugins/call_trigger_service_node.cpp b/panther_manager/src/plugins/call_trigger_service_node.cpp new file mode 100644 index 000000000..2206d55db --- /dev/null +++ b/panther_manager/src/plugins/call_trigger_service_node.cpp @@ -0,0 +1,26 @@ +#include +namespace panther_manager +{ + +bool CallTriggerService::setRequest(typename Request::SharedPtr& /*request*/) +{ + return true; +} + +BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::SharedPtr& response) +{ + if (!response->success) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); + return BT::NodeStatus::FAILURE; + } + RCLCPP_DEBUG_STREAM(node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallTriggerService, "CallTriggerService"); From 85f4d659b74566c1314401eea1dfc75853f4dd55 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 19 Feb 2024 12:13:14 +0000 Subject: [PATCH 05/35] Added full call_set_bool plugin test Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 36 ++++ .../plugins/call_set_bool_service_node.hpp | 1 - .../call_set_led_animation_service_node.hpp | 34 ++++ .../plugins/call_set_bool_service_node.cpp | 7 +- .../call_set_led_animation_service_node.cpp | 43 +++++ .../panther_manager_plugin_test_utils.hpp | 68 +++++++ .../src/panther_manager_plugin_test_utils.cpp | 99 ++++++++++ .../test/plugins/test_set_bool_plugin.cpp | 177 ++++++++++++++++++ 8 files changed, 458 insertions(+), 7 deletions(-) create mode 100644 panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp create mode 100644 panther_manager/src/plugins/call_set_led_animation_service_node.cpp create mode 100644 panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp create mode 100644 panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp create mode 100644 panther_manager/test/plugins/test_set_bool_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 47f595ed9..ea5287aff 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -12,6 +12,7 @@ set(PACKAGE_INCLUDE_DEPENDS ament_index_cpp behaviortree_cpp std_srvs + panther_msgs ) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) @@ -27,11 +28,46 @@ list(APPEND plugin_libs call_set_bool_service_bt_node) add_library(call_trigger_service_bt_node SHARED src/plugins/call_trigger_service_node.cpp) list(APPEND plugin_libs call_trigger_service_bt_node) +add_library(call_set_led_animation_service_bt_node SHARED src/plugins/call_set_led_animation_service_node.cpp) +list(APPEND plugin_libs call_set_led_animation_service_bt_node) + foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) endforeach() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + add_library(${PROJECT_NAME}_test_plugin_utils test/plugins/src/panther_manager_plugin_test_utils.cpp) + target_include_directories(${PROJECT_NAME}_test_plugin_utils PUBLIC test/plugins/include) + + target_link_libraries(${PROJECT_NAME}_test_plugin_utils + call_set_bool_service_bt_node + call_trigger_service_bt_node + call_set_led_animation_service_bt_node + ) + + ament_target_dependencies( + ${PROJECT_NAME}_test_plugin_utils ${PACKAGE_INCLUDE_DEPENDS} + ) + + ament_add_gtest( + ${PROJECT_NAME}_test_set_bool_plugin + test/plugins/test_set_bool_plugin.cpp + ) + + target_link_libraries(${PROJECT_NAME}_test_set_bool_plugin + ${PROJECT_NAME}_test_plugin_utils + ) + + ament_target_dependencies( + ${PROJECT_NAME}_test_set_bool_plugin ${PACKAGE_INCLUDE_DEPENDS} + ) +endif() + + install(TARGETS DESTINATION lib/${PROJECT_NAME} ) diff --git a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp index fee652eee..f40f649d4 100644 --- a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp @@ -23,7 +23,6 @@ class CallSetBoolService : public BT::RosServiceNode virtual bool setRequest(typename Request::SharedPtr& request) override; virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; - virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode /*error*/) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp new file mode 100644 index 000000000..6ea30c123 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp @@ -0,0 +1,34 @@ +#ifndef PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ + +#include +#include +#include + +namespace panther_manager +{ + +class CallSetLedAnimationService : public BT::RosServiceNode +{ +public: + CallSetLedAnimationService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("id", "animation ID"), + BT::InputPort("param", "optional parameter"), + BT::InputPort("repeating", "indicates if animation should repeat"), + }); + } + + virtual bool setRequest(typename Request::SharedPtr& request) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ diff --git a/panther_manager/src/plugins/call_set_bool_service_node.cpp b/panther_manager/src/plugins/call_set_bool_service_node.cpp index 411c10f4e..cd51f8e37 100644 --- a/panther_manager/src/plugins/call_set_bool_service_node.cpp +++ b/panther_manager/src/plugins/call_set_bool_service_node.cpp @@ -6,6 +6,7 @@ bool CallSetBoolService::setRequest(typename Request::SharedPtr& request) { if (!getInput("data", request->data)) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [data]"); return false; } return true; @@ -24,12 +25,6 @@ BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::S return BT::NodeStatus::SUCCESS; } -BT::NodeStatus CallSetBoolService::onFailure(BT::ServiceNodeErrorCode /*error*/) -{ - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [data]"); - return BT::NodeStatus::FAILURE; -} - } // namespace panther_manager #include diff --git a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp new file mode 100644 index 000000000..fe268959e --- /dev/null +++ b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp @@ -0,0 +1,43 @@ +#include +namespace panther_manager +{ + +bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr& request) +{ + if (!getInput("id", request->animation.id)) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [id]"); + return false; + } + if (!getInput("param", request->animation.param)) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [param]"); + return false; + } + + if (!getInput("repeating", request->repeating)) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [repeating]"); + return false; + } + + return true; +} + +BT::NodeStatus CallSetLedAnimationService::onResponseReceived(const typename Response::SharedPtr& response) +{ + if (!response->success) + { + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); + return BT::NodeStatus::FAILURE; + } + RCLCPP_DEBUG_STREAM(node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallSetLedAnimationService, "CallSetLedAnimationService"); diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp new file mode 100644 index 000000000..3de9ab4e2 --- /dev/null +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ +#define PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace panther_manager_plugin_test +{ +class PantherManagerPluginTestUtils +{ +public: + PantherManagerPluginTestUtils(); + std::string BuildBehaviorTree(const std::string& plugin_name, + const std::map& name_and_data_map); + + BT::Tree &CreateTree(const std::string& plugin_name, const std::map& name_and_data_map); + BT::BehaviorTreeFactory& GetFactory(); + + void Start(); + void Stop(); + + void CreateSetBoolServiceServer( + std::function + service_callback); + void CreateTriggerServiceServer(std::function service_callback); + void CreateSetLEDAnimationServiceServer(std::function service_callback); + +private: + rclcpp::Node::SharedPtr bt_node_; + BT::BehaviorTreeFactory factory_; + BT::Tree tree_; + + rclcpp::Node::SharedPtr service_server_node_; + rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; + + rclcpp::Service::SharedPtr set_bool_server_; + std::unique_ptr executor_thread_; + + void spin_executor(); +}; +} // namespace panther_manager_plugin_test + +#endif // PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp new file mode 100644 index 000000000..296764317 --- /dev/null +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -0,0 +1,99 @@ +#include + +namespace panther_manager_plugin_test +{ + +PantherManagerPluginTestUtils::PantherManagerPluginTestUtils() +{ +} + +std::string PantherManagerPluginTestUtils::BuildBehaviorTree( + const std::string& plugin_name, const std::map& name_and_data_map) +{ + std::stringstream bt; + auto header = R"( + + + + )"; + + auto footer = R"( + + + + )"; + + bt << header << std::endl; + + for (auto const& [name, value] : name_and_data_map) + { + bt << "\t\t\t\t<" << plugin_name << " name=\"" << name << "\" service_name=\"" << name << "\" data=\"" << value + << "\" />" << std::endl; + } + + bt << footer; + + return bt.str(); +} + +BT::Tree &PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, + const std::map& name_and_data_map) +{ + auto xml_text = BuildBehaviorTree(plugin_name, name_and_data_map); + tree_ = factory_.createTreeFromText(xml_text); + return tree_; +} + +BT::BehaviorTreeFactory& PantherManagerPluginTestUtils::GetFactory() +{ + return factory_; +} + +void PantherManagerPluginTestUtils::Start() +{ + rclcpp::init(0, nullptr); + bt_node_ = std::make_shared("test_panther_manager_node"); + BT::RosNodeParams params; + params.nh = bt_node_; + + factory_.registerNodeType("CallSetBoolService", params); + factory_.registerNodeType("CallTriggerService", params); + factory_.registerNodeType("CallSetLedAnimationService", params); +} + +void PantherManagerPluginTestUtils::Stop() +{ + bt_node_.reset(); + rclcpp::shutdown(); + if (executor_thread_) + { + executor_.reset(); + executor_thread_->join(); + } +} + +void PantherManagerPluginTestUtils::CreateSetBoolServiceServer( + std::function + service_callback) +{ + service_server_node_ = std::make_shared("test_set_bool_service"); + set_bool_server_ = service_server_node_->create_service("set_bool", service_callback); + executor_ = std::make_unique(); + executor_->add_node(service_server_node_); + executor_thread_ = std::make_unique([this]() { executor_->spin(); }); +} + +void PantherManagerPluginTestUtils::CreateTriggerServiceServer(std::function service_callback) +{ +} + +void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer(std::function service_callback) +{ +} + +void PantherManagerPluginTestUtils::spin_executor() +{ + executor_->spin(); +} + +} // namespace panther_manager_plugin_test diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_set_bool_plugin.cpp new file mode 100644 index 000000000..1d4c2cc22 --- /dev/null +++ b/panther_manager/test/plugins/test_set_bool_plugin.cpp @@ -0,0 +1,177 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include +#include + +#include + +#include + +void ServiceFailedCallback(const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) +{ + response->message = "Failed callback pass!"; + response->success = false; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); +} + +void ServiceSuccessCallbackCheckTrueValue(const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); + + if (request->data != true) + { + FAIL() << "Service data should be true!"; + } +} + +void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); + + if (request->data != false) + { + FAIL() << "Service data should be false!"; + } +} + +TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) +{ + std::map services = { { "test_set_bool_service", "true" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + + ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", services); }); + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) +{ + std::map services = { { "test_set_bool_service", "true" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", services); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) +{ + std::map services = { { "set_bool", "true" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetBoolService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Found set_bool service but shouldn't!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) +{ + std::map services = { { "set_bool", "true" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetBoolService", services); + + test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call set_bool service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) +{ + std::map services = { { "set_bool", "false" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetBoolService", services); + + test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call set_bool service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) +{ + std::map services = { { "set_bool", "false" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetBoolService", services); + + test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Cannot call set_bool service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetBoolService, wrong_service_value_defined) +{ + std::map services = { { "set_bool", "wrong_bool" } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetBoolService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Wrong value is parsed as good value!"; + } + + test_utils.Stop(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From bed1fbe589f2b524451c464581aea0726150b1f8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 19 Feb 2024 12:34:16 +0000 Subject: [PATCH 06/35] Added trigger tests Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 15 +- .../panther_manager_plugin_test_utils.hpp | 12 +- .../src/panther_manager_plugin_test_utils.cpp | 58 ++++++-- .../test/plugins/test_trigger_plugin.cpp | 128 ++++++++++++++++++ 4 files changed, 196 insertions(+), 17 deletions(-) create mode 100644 panther_manager/test/plugins/test_trigger_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index ea5287aff..494a1a65a 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -57,14 +57,19 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_set_bool_plugin test/plugins/test_set_bool_plugin.cpp ) + list(APPEND plugin_tests ${PROJECT_NAME}_test_set_bool_plugin) - target_link_libraries(${PROJECT_NAME}_test_set_bool_plugin - ${PROJECT_NAME}_test_plugin_utils + ament_add_gtest( + ${PROJECT_NAME}_test_trigger_plugin + test/plugins/test_trigger_plugin.cpp ) + list(APPEND plugin_tests ${PROJECT_NAME}_test_trigger_plugin) - ament_target_dependencies( - ${PROJECT_NAME}_test_set_bool_plugin ${PACKAGE_INCLUDE_DEPENDS} - ) + + foreach(bt_plugin_test ${plugin_tests}) + target_link_libraries(${bt_plugin_test} ${PROJECT_NAME}_test_plugin_utils) + ament_target_dependencies(${bt_plugin_test} ${PACKAGE_INCLUDE_DEPENDS}) + endforeach() endif() diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 3de9ab4e2..086c83aae 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -34,11 +34,13 @@ namespace panther_manager_plugin_test class PantherManagerPluginTestUtils { public: - PantherManagerPluginTestUtils(); std::string BuildBehaviorTree(const std::string& plugin_name, const std::map& name_and_data_map); + std::string BuildBehaviorTree(const std::string& plugin_name, + const std::vector& names); - BT::Tree &CreateTree(const std::string& plugin_name, const std::map& name_and_data_map); + BT::Tree& CreateTree(const std::string& plugin_name, const std::map& name_and_data_map); + BT::Tree& CreateTree(const std::string& plugin_name, const std::vector& names); BT::BehaviorTreeFactory& GetFactory(); void Start(); @@ -47,8 +49,8 @@ class PantherManagerPluginTestUtils void CreateSetBoolServiceServer( std::function service_callback); - void CreateTriggerServiceServer(std::function service_callback); - void CreateSetLEDAnimationServiceServer(std::function service_callback); + void CreateTriggerServiceServer(std::function service_callback); + void CreateSetLEDAnimationServiceServer(std::function service_callback); private: rclcpp::Node::SharedPtr bt_node_; @@ -59,6 +61,8 @@ class PantherManagerPluginTestUtils rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; rclcpp::Service::SharedPtr set_bool_server_; + rclcpp::Service::SharedPtr trigger_server_; + rclcpp::Service::SharedPtr set_led_animation_server_; std::unique_ptr executor_thread_; void spin_executor(); diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index 296764317..234fa5270 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -3,10 +3,6 @@ namespace panther_manager_plugin_test { -PantherManagerPluginTestUtils::PantherManagerPluginTestUtils() -{ -} - std::string PantherManagerPluginTestUtils::BuildBehaviorTree( const std::string& plugin_name, const std::map& name_and_data_map) { @@ -36,14 +32,50 @@ std::string PantherManagerPluginTestUtils::BuildBehaviorTree( return bt.str(); } -BT::Tree &PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, - const std::map& name_and_data_map) +std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& plugin_name, + const std::vector& names) +{ + std::stringstream bt; + auto header = R"( + + + + )"; + + auto footer = R"( + + + + )"; + + bt << header << std::endl; + + for (auto const& name : names) + { + bt << "\t\t\t\t<" << plugin_name << " name=\"" << name << "\" service_name=\"" << name << "\" />" << std::endl; + } + + bt << footer; + + return bt.str(); +} + +BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, + const std::map& name_and_data_map) { auto xml_text = BuildBehaviorTree(plugin_name, name_and_data_map); tree_ = factory_.createTreeFromText(xml_text); return tree_; } +BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, + const std::vector& names) +{ + auto xml_text = BuildBehaviorTree(plugin_name, names); + tree_ = factory_.createTreeFromText(xml_text); + return tree_; +} + BT::BehaviorTreeFactory& PantherManagerPluginTestUtils::GetFactory() { return factory_; @@ -83,11 +115,21 @@ void PantherManagerPluginTestUtils::CreateSetBoolServiceServer( executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::CreateTriggerServiceServer(std::function service_callback) +void PantherManagerPluginTestUtils::CreateTriggerServiceServer( + std::function + service_callback) { + service_server_node_ = std::make_shared("test_set_bool_service"); + trigger_server_ = service_server_node_->create_service("trigger", service_callback); + executor_ = std::make_unique(); + executor_->add_node(service_server_node_); + executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer(std::function service_callback) +void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( + std::function + service_callback) { } diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_trigger_plugin.cpp new file mode 100644 index 000000000..7e215b7be --- /dev/null +++ b/panther_manager/test/plugins/test_trigger_plugin.cpp @@ -0,0 +1,128 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +#include +#include +#include + +#include + +#include + +void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->message = "Failed callback pass!"; + response->success = false; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); +} + +void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); +} + + + +TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) +{ + std::vector services = { "test_trigger_service"}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + + ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", services); }); + test_utils.Stop(); +} + +TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) +{ + std::vector services = {"test_trigger_service" }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", services); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) +{ + std::vector services = { "trigger"}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Found trigger service but shouldn't!"; + } + + test_utils.Stop(); +} + +TEST(TestCallTriggerService, good_trigger_call_service_success) +{ + std::vector services = { "trigger"}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", services); + + test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call trigger service!"; + } + + test_utils.Stop(); +} + + +TEST(TestCallTriggerService, wrong_trigger_call_service_failure) +{ + std::vector services = {"trigger"}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", services); + + test_utils.CreateTriggerServiceServer(ServiceFailedCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Cannot call trigger service!"; + } + + test_utils.Stop(); +} + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 7a5d74c422516b856cef3e86c962833395938c3e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 19 Feb 2024 14:14:26 +0000 Subject: [PATCH 07/35] Added tests for set animation plugin Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 5 + .../call_set_led_animation_service_node.cpp | 6 +- .../panther_manager_plugin_test_utils.hpp | 28 ++- .../src/panther_manager_plugin_test_utils.cpp | 67 ++--- .../test/plugins/test_set_bool_plugin.cpp | 10 +- .../plugins/test_set_led_animation_plugin.cpp | 235 ++++++++++++++++++ 6 files changed, 310 insertions(+), 41 deletions(-) create mode 100644 panther_manager/test/plugins/test_set_led_animation_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 494a1a65a..96de31600 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -65,6 +65,11 @@ if(BUILD_TESTING) ) list(APPEND plugin_tests ${PROJECT_NAME}_test_trigger_plugin) + ament_add_gtest( + ${PROJECT_NAME}_test_set_led_animation_plugin + test/plugins/test_set_led_animation_plugin.cpp + ) + list(APPEND plugin_tests ${PROJECT_NAME}_test_set_led_animation_plugin) foreach(bt_plugin_test ${plugin_tests}) target_link_libraries(${bt_plugin_test} ${PROJECT_NAME}_test_plugin_utils) diff --git a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp index fe268959e..4bde77ab8 100644 --- a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp +++ b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp @@ -4,11 +4,15 @@ namespace panther_manager bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr& request) { - if (!getInput("id", request->animation.id)) + unsigned animation_id; + if (!getInput("id", animation_id)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [id]"); return false; } + + request->animation.id = animation_id; + if (!getInput("param", request->animation.param)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [param]"); diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 086c83aae..eb839ebb2 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -31,16 +31,22 @@ namespace panther_manager_plugin_test { + +typedef std::array SetLEDAnimationTestUtils; + class PantherManagerPluginTestUtils { public: std::string BuildBehaviorTree(const std::string& plugin_name, const std::map& name_and_data_map); + std::string BuildBehaviorTree(const std::string& plugin_name, const std::vector& names); std::string BuildBehaviorTree(const std::string& plugin_name, - const std::vector& names); + const std::map& animation_params); BT::Tree& CreateTree(const std::string& plugin_name, const std::map& name_and_data_map); BT::Tree& CreateTree(const std::string& plugin_name, const std::vector& names); + BT::Tree& CreateTree(const std::string& plugin_name, + const std::map& animation_params); BT::BehaviorTreeFactory& GetFactory(); void Start(); @@ -49,8 +55,12 @@ class PantherManagerPluginTestUtils void CreateSetBoolServiceServer( std::function service_callback); - void CreateTriggerServiceServer(std::function service_callback); - void CreateSetLEDAnimationServiceServer(std::function service_callback); + void CreateTriggerServiceServer( + std::function + service_callback); + void CreateSetLEDAnimationServiceServer(std::function + service_callback); private: rclcpp::Node::SharedPtr bt_node_; @@ -66,6 +76,18 @@ class PantherManagerPluginTestUtils std::unique_ptr executor_thread_; void spin_executor(); + + std::string header_ = R"( + + + + )"; + + std::string footer_ = R"( + + + + )"; }; } // namespace panther_manager_plugin_test diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index 234fa5270..3b6678410 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -7,27 +7,16 @@ std::string PantherManagerPluginTestUtils::BuildBehaviorTree( const std::string& plugin_name, const std::map& name_and_data_map) { std::stringstream bt; - auto header = R"( - - - - )"; - auto footer = R"( - - - - )"; - - bt << header << std::endl; + bt << header_ << std::endl; for (auto const& [name, value] : name_and_data_map) { - bt << "\t\t\t\t<" << plugin_name << " name=\"" << name << "\" service_name=\"" << name << "\" data=\"" << value + bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" data=\"" << value << "\" />" << std::endl; } - bt << footer; + bt << footer_; return bt.str(); } @@ -36,26 +25,33 @@ std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& const std::vector& names) { std::stringstream bt; - auto header = R"( - - - - )"; - - auto footer = R"( - - - - )"; - bt << header << std::endl; + bt << header_ << std::endl; for (auto const& name : names) { - bt << "\t\t\t\t<" << plugin_name << " name=\"" << name << "\" service_name=\"" << name << "\" />" << std::endl; + bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" />" << std::endl; } - bt << footer; + bt << footer_; + + return bt.str(); +} + +std::string PantherManagerPluginTestUtils::BuildBehaviorTree( + const std::string& plugin_name, const std::map& animation_params) +{ + std::stringstream bt; + + bt << header_ << std::endl; + + for (auto const& [name, param] : animation_params) + { + bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" id=\"" << param[0] << "\" param=\"" + << param[1] << "\" repeating=\"" << param[2] << "\" />" << std::endl; + } + + bt << footer_; return bt.str(); } @@ -76,6 +72,14 @@ BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_na return tree_; } +BT::Tree& PantherManagerPluginTestUtils::CreateTree( + const std::string& plugin_name, const std::map& animation_params) +{ + auto xml_text = BuildBehaviorTree(plugin_name, animation_params); + tree_ = factory_.createTreeFromText(xml_text); + return tree_; +} + BT::BehaviorTreeFactory& PantherManagerPluginTestUtils::GetFactory() { return factory_; @@ -119,7 +123,7 @@ void PantherManagerPluginTestUtils::CreateTriggerServiceServer( std::function service_callback) { - service_server_node_ = std::make_shared("test_set_bool_service"); + service_server_node_ = std::make_shared("test_trigger_service"); trigger_server_ = service_server_node_->create_service("trigger", service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); @@ -131,6 +135,11 @@ void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( panther_msgs::srv::SetLEDAnimation::Response::SharedPtr)> service_callback) { + service_server_node_ = std::make_shared("test_set_led_animation_service"); + set_led_animation_server_ = service_server_node_->create_service("set_led_animation", service_callback); + executor_ = std::make_unique(); + executor_->add_node(service_server_node_); + executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } void PantherManagerPluginTestUtils::spin_executor() diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_set_bool_plugin.cpp index 1d4c2cc22..98a892ff9 100644 --- a/panther_manager/test/plugins/test_set_bool_plugin.cpp +++ b/panther_manager/test/plugins/test_set_bool_plugin.cpp @@ -38,10 +38,7 @@ void ServiceSuccessCallbackCheckTrueValue(const std_srvs::srv::SetBool::Request: response->success = true; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); - if (request->data != true) - { - FAIL() << "Service data should be true!"; - } + EXPECT_EQ(request->data, true); } void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request::SharedPtr request, @@ -51,10 +48,7 @@ void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request response->success = true; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); - if (request->data != false) - { - FAIL() << "Service data should be false!"; - } + EXPECT_EQ(request->data, false); } TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) diff --git a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp new file mode 100644 index 000000000..8f5db5acf --- /dev/null +++ b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp @@ -0,0 +1,235 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include +#include + +#include + +#include + +void ServiceFailedCallback(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +{ + response->message = "Failed callback pass!"; + response->success = false; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); +} + +void ServiceSuccessCallbackCheckRepeatingTrueValue(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); + + EXPECT_EQ(request->repeating, true); +} + +void ServiceSuccessCallbackCheckRepeatingFalseValue(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); + + EXPECT_EQ(request->repeating, false); +} + +void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +{ + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); + + EXPECT_EQ(request->animation.id, 5u); +} + +TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service_plugin) +{ + std::map services = { { "set_led_animation", + { "0", "", "true" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", services); }); + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) +{ + std::map services = { { "set_led_animation", + { "0", "", "true" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongCallSetLedAnimationService", services); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_service_server_not_initialized) +{ + std::map services = { { "set_led_animation", + { "0", "", "true" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Found set_led_animation service but shouldn't!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_true_repeating_value) +{ + std::map services = { { "set_led_animation", + { "0", "", "true" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call set_led_animation service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_false_repeating_value) +{ + std::map services = { { "set_led_animation", + { "0", "", "false" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call set_led_animation service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) +{ + std::map services = { { "set_led_animation", + { "5", "", "false" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call set_led_animation service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) +{ + std::map services = { { "set_led_animation", + { "0", "", "true" } } }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Cannot call set_led_animation service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) +{ + std::map services = { { "set_led_animation", + { "0", "", "wrong_bool" } } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Wrong value is parsed as good value!"; + } + + test_utils.Stop(); +} + +TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) +{ + std::map services = { { "set_led_animation", + { "-5", "", "true" } } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Wrong value is parsed as good value!"; + } + + test_utils.Stop(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From f933e45a591402030acc1b6c9cf25f9ed8792239 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 19 Feb 2024 14:15:09 +0000 Subject: [PATCH 08/35] Added explicite casts Signed-off-by: Jakub Delicat --- .../src/plugins/call_set_led_animation_service_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp index 4bde77ab8..c3f205270 100644 --- a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp +++ b/panther_manager/src/plugins/call_set_led_animation_service_node.cpp @@ -11,7 +11,7 @@ bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr& request return false; } - request->animation.id = animation_id; + request->animation.id = static_cast(animation_id); if (!getInput("param", request->animation.param)) { From 35ddb1e45e65e948abbbc35bf8d854d5318ddad3 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Feb 2024 10:10:19 +0000 Subject: [PATCH 09/35] Moved pluigns to actions | added shutdown plugins --- panther_manager/CMakeLists.txt | 6 +- .../call_set_bool_service_node.hpp | 14 + .../call_set_led_animation_service_node.hpp | 14 + .../call_trigger_service_node.hpp | 14 + .../action/shutdown_hosts_from_file_node.hpp | 53 +++ .../plugins/action/shutdown_hosts_node.hpp | 183 ++++++++++ .../action/shutdown_single_host_node.hpp | 57 +++ .../plugins/action/signal_shutdown_node.hpp | 48 +++ .../panther_manager/plugins/shutdown_host.hpp | 327 ++++++++++++++++++ panther_manager/package.xml | 1 + .../call_set_bool_service_node.cpp | 2 +- .../call_set_led_animation_service_node.cpp | 2 +- .../call_trigger_service_node.cpp | 2 +- .../action/shutdown_hosts_from_file_node.cpp | 68 ++++ .../action/shutdown_single_host_node.cpp | 57 +++ .../plugins/action/signal_shutdown_node.cpp | 30 ++ .../panther_manager_plugin_test_utils.hpp | 6 +- .../test/plugins/test_set_bool_plugin.cpp | 2 +- .../plugins/test_set_led_animation_plugin.cpp | 2 +- .../test/plugins/test_trigger_plugin.cpp | 4 +- 20 files changed, 878 insertions(+), 14 deletions(-) rename panther_manager/include/panther_manager/plugins/{ => action}/call_set_bool_service_node.hpp (61%) rename panther_manager/include/panther_manager/plugins/{ => action}/call_set_led_animation_service_node.hpp (66%) rename panther_manager/include/panther_manager/plugins/{ => action}/call_trigger_service_node.hpp (59%) create mode 100644 panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/shutdown_host.hpp rename panther_manager/src/plugins/{ => action}/call_set_bool_service_node.cpp (92%) rename panther_manager/src/plugins/{ => action}/call_set_led_animation_service_node.cpp (94%) rename panther_manager/src/plugins/{ => action}/call_trigger_service_node.cpp (91%) create mode 100644 panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp create mode 100644 panther_manager/src/plugins/action/shutdown_single_host_node.cpp create mode 100644 panther_manager/src/plugins/action/signal_shutdown_node.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 96de31600..28fc000b1 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -22,13 +22,13 @@ endforeach() include_directories(include) add_library(behaviortree_ros2 src/bt_ros2.cpp) -add_library(call_set_bool_service_bt_node SHARED src/plugins/call_set_bool_service_node.cpp) +add_library(call_set_bool_service_bt_node SHARED src/plugins/action/call_set_bool_service_node.cpp) list(APPEND plugin_libs call_set_bool_service_bt_node) -add_library(call_trigger_service_bt_node SHARED src/plugins/call_trigger_service_node.cpp) +add_library(call_trigger_service_bt_node SHARED src/plugins/action/call_trigger_service_node.cpp) list(APPEND plugin_libs call_trigger_service_bt_node) -add_library(call_set_led_animation_service_bt_node SHARED src/plugins/call_set_led_animation_service_node.cpp) +add_library(call_set_led_animation_service_bt_node SHARED src/plugins/action/call_set_led_animation_service_node.cpp) list(APPEND plugin_libs call_set_led_animation_service_bt_node) foreach(bt_plugin ${plugin_libs}) diff --git a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp similarity index 61% rename from panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp rename to panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index f40f649d4..cbd8215e0 100644 --- a/panther_manager/include/panther_manager/plugins/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp similarity index 66% rename from panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp rename to panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 6ea30c123..9cd59cd9e 100644 --- a/panther_manager/include/panther_manager/plugins/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp similarity index 59% rename from panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp rename to panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp index 6c6a77a85..120df2008 100644 --- a/panther_manager/include/panther_manager/plugins/call_trigger_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp new file mode 100644 index 000000000..9923b1740 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include + +namespace panther_manager +{ + +class ShutdownHostsFromFile : public ShutdownHosts +{ +public: + ShutdownHostsFromFile(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHosts(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"), + }; + } + +private: + void update_hosts(std::vector> & hosts) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp new file mode 100644 index 000000000..386094007 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp @@ -0,0 +1,183 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace panther_manager +{ + +class ShutdownHosts : public BT::StatefulActionNode +{ +public: + explicit ShutdownHosts(const std::string& name, const BT::NodeConfig& conf) : BT::StatefulActionNode(name, conf) + { + // TODO: @delihus What is the name of ros::this_node? + node_name_ = name; + logger_ = std::make_shared({ rclcpp::get_logger("node_name_") }); + } + + virtual ~ShutdownHosts() = default; + + // method to be implemented by user + virtual void update_hosts(std::vector>& hosts) = 0; + + // method that can be overriden by user + virtual BT::NodeStatus post_process() + { + // return success only when all hosts succeeded + if (failed_hosts_.size() == 0) + { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; + } + + std::string get_node_name() const + { + return node_name_; + } + std::vector const get_failed_hosts() + { + return failed_hosts_; + } + +private: + int check_host_index_ = 0; + std::string node_name_; + std::vector> hosts_; + std::vector hosts_to_check_; + std::vector skipped_hosts_; + std::vector succeeded_hosts_; + std::vector failed_hosts_; + rclcpp::Logger::SharedPtr logger_; + + BT::NodeStatus onStart() + { + update_hosts(hosts_); + remove_duplicate_hosts(hosts_); + if (hosts_.size() <= 0) + { + RCLCPP_ERROR_STREAM(&logger_, "Hosts list is empty! Check configuration!"); + return BT::NodeStatus::FAILURE; + } + hosts_to_check_.resize(hosts_.size()); + std::iota(hosts_to_check_.begin(), hosts_to_check_.end(), 0); + return BT::NodeStatus::RUNNING; + } + + BT::NodeStatus onRunning() + { + if (hosts_to_check_.size() <= 0) + { + return post_process(); + } + + if (check_host_index_ >= hosts_to_check_.size()) + { + check_host_index_ = 0; + } + + auto host_index = hosts_to_check_[check_host_index_]; + auto host = hosts_[host_index]; + host->call(); + + switch (host->get_state()) + { + case ShutdownHostState::RESPONSE_RECEIVED: + RCLCPP_INFO_STREAM(&logger_, "Device at: " << host->get_ip() << " response:\n" << host->get_response()); + + check_host_index_++; + break; + + case ShutdownHostState::SUCCESS: + RCLCPP_INFO_STREAM(&logger_, "Successfully shutdown device at: " << host->get_ip()); + succeeded_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + case ShutdownHostState::FAILURE: + ROS_WARN("[%s] Failed to shutdown device at: %s. Error: %s", node_name_.c_str(), host->get_ip().c_str(), + host->get_error().c_str()); + failed_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + case ShutdownHostState::SKIPPED: + RCLCPP_WARN_STREAM(&logger_, "Device at: " << host->get_ip() << " not available, skipping..."); + + skipped_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + default: + check_host_index_++; + break; + } + + return BT::NodeStatus::RUNNING; + } + + void remove_duplicate_hosts(std::vector>& hosts) + { + std::set seen; + + hosts.erase(std::remove_if(hosts.begin(), hosts.end(), + [&](const std::shared_ptr& host) { + if (!seen.count(*host)) + { + seen.insert(*host); + return false; + } + else + { + RCLCPP_WARN_STREAM(&logger_, "Found duplicate host: " << host->get_ip() + << " Processing only the " + "first " + "occurrence."); + return true; + } + }), + hosts.end()); + } + + void onHalted() + { + for (auto& host : hosts_) + { + host->close_connection(); + } + } +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp new file mode 100644 index 000000000..1b0cfdc6b --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace panther_manager +{ + +class ShutdownSingleHost : public ShutdownHosts +{ +public: + ShutdownSingleHost(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHosts(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("ip", "ip of the host to shutdown"), + BT::InputPort("user", "user to log into while executing shutdown command"), + BT::InputPort("port", "SSH communication port"), + BT::InputPort("command", "command to execute on shutdown"), + BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), + BT::InputPort( + "ping_for_success", "ping host unitl it is not available or timeout is reached"), + }; + } + +private: + void update_hosts(std::vector> & hosts) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp new file mode 100644 index 000000000..b62e51e55 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ +#define PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ + +#include + +#include +#include +#include + +namespace panther_manager +{ + +class SignalShutdown : public BT::SyncActionNode +{ +public: + explicit SignalShutdown(const std::string & name, const BT::NodeConfig & conf) + : BT::SyncActionNode(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("reason", "", "reason to shutdown robot"), + }; + } + +private: + virtual BT::NodeStatus tick() override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp new file mode 100644 index 000000000..ad11f12ab --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -0,0 +1,327 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace panther_manager +{ + +enum class ShutdownHostState +{ + IDLE = 0, + COMMAND_EXECUTED, + RESPONSE_RECEIVED, + PINGING, + SKIPPED, + SUCCESS, + FAILURE, +}; + +class ShutdownHost +{ +public: + // default constructor + ShutdownHost() + : ip_("") + , user_("") + , port_(22) + , command_("") + , timeout_(5000) + , ping_for_success_(true) + , hash_(std::hash{}("")) + { + } + ShutdownHost(const std::string ip, const std::string user, const int port = 22, + const std::string command = "sudo shutdown now", const float timeout = 5.0, + const bool ping_for_success = true) + : ip_(ip) + , user_(user) + , port_(port) + , command_(command) + , timeout_(static_cast(timeout * 1000)) + , ping_for_success_(ping_for_success) + , hash_(std::hash{}(ip + user + std::to_string(port))) + , state_(ShutdownHostState::IDLE) + { + } + + ~ShutdownHost() + { + } + + void call() + { + switch (state_) + { + case ShutdownHostState::IDLE: + if (!is_available()) + { + state_ = ShutdownHostState::SKIPPED; + break; + } + + try + { + request_shutdown(); + } + catch (std::runtime_error err) + { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = err.what(); + break; + } + state_ = ShutdownHostState::COMMAND_EXECUTED; + break; + + case ShutdownHostState::COMMAND_EXECUTED: + try + { + if (update_response()) + { + break; + } + } + catch (std::runtime_error err) + { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = err.what(); + break; + } + state_ = ShutdownHostState::RESPONSE_RECEIVED; + break; + + case ShutdownHostState::RESPONSE_RECEIVED: + state_ = ShutdownHostState::PINGING; + break; + case ShutdownHostState::PINGING: + if (ping_for_success_ ? !is_available() : true) + { + state_ = ShutdownHostState::SUCCESS; + break; + } + if (timeout_exceeded()) + { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = "Timeout exceeded"; + } + break; + + default: + break; + } + } + + bool is_available() const + { + return system(("ping -c 1 -w 1 " + ip_ + " > /dev/null").c_str()) == 0; + } + + void close_connection() + { + if (ssh_channel_is_closed(channel_)) + { + return; + } + + ssh_channel_send_eof(channel_); + ssh_channel_close(channel_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + } + + int get_port() const + { + return port_; + } + + std::string get_ip() const + { + return ip_; + } + + std::string get_user() const + { + return user_; + } + + std::string get_command() const + { + return command_; + } + + std::string get_error() const + { + return failure_reason_; + } + + std::string get_response() const + { + return output_; + } + + ShutdownHostState get_state() const + { + return state_; + } + + bool operator==(const ShutdownHost& other) const + { + return hash_ == other.hash_; + } + + bool operator!=(const ShutdownHost& other) const + { + return hash_ != other.hash_; + } + + bool operator<(const ShutdownHost& other) const + { + return hash_ < other.hash_; + } + +private: + const std::string ip_; + const std::string user_; + const std::string command_; + const std::size_t hash_; + const int port_; + const bool ping_for_success_; + const std::chrono::milliseconds timeout_; + + char buffer_[1024]; + const int verbosity_ = SSH_LOG_NOLOG; + int nbytes_; + std::string output_; + std::string failure_reason_; + std::chrono::time_point command_time_; + ShutdownHostState state_; + + ssh_session session_; + ssh_channel channel_; + + void request_shutdown() + { + ssh_execute_command(command_); + command_time_ = std::chrono::steady_clock::now(); + } + + bool update_response() + { + if (!is_available()) + { + close_connection(); + throw std::runtime_error("Lost connection"); + } + + if (!ssh_channel_is_open(channel_)) + { + throw std::runtime_error("Channel closed"); + } + + if (timeout_exceeded()) + { + close_connection(); + throw std::runtime_error("Timeout exceeded"); + } + + if ((nbytes_ = ssh_channel_read_nonblocking(channel_, buffer_, sizeof(buffer_), 0)) >= 0) + { + output_.append(buffer_, nbytes_); + return true; + } + close_connection(); + return false; + } + + bool timeout_exceeded() + { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - command_time_); + + return elapsed > timeout_ && is_available(); + } + + void ssh_execute_command(const std::string& command) + { + session_ = ssh_new(); + if (session_ == NULL) + { + throw std::runtime_error("Failed to open session"); + }; + + ssh_options_set(session_, SSH_OPTIONS_HOST, ip_.c_str()); + ssh_options_set(session_, SSH_OPTIONS_USER, user_.c_str()); + ssh_options_set(session_, SSH_OPTIONS_PORT, &port_); + ssh_options_set(session_, SSH_OPTIONS_LOG_VERBOSITY, &verbosity_); + + if (ssh_connect(session_) != SSH_OK) + { + std::string err = ssh_get_error(session_); + ssh_free(session_); + throw std::runtime_error("Error connecting to host: " + err); + } + + if (ssh_userauth_publickey_auto(session_, NULL, NULL) != SSH_AUTH_SUCCESS) + { + std::string err = ssh_get_error(session_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Error authenticating with public key: " + err); + } + + channel_ = ssh_channel_new(session_); + if (channel_ == NULL) + { + std::string err = ssh_get_error(session_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to create ssh channel: " + err); + } + + if (ssh_channel_open_session(channel_) != SSH_OK) + { + std::string err = ssh_get_error(session_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to open ssh channel: " + err); + } + + if (ssh_channel_request_exec(channel_, command.c_str()) != SSH_OK) + { + std::string err = ssh_get_error(session_); + ssh_channel_close(channel_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to execute ssh command: " + err); + } + } +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOST_NODE_HPP_ diff --git a/panther_manager/package.xml b/panther_manager/package.xml index d62b4345d..4030b1f50 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -19,6 +19,7 @@ rclcpp_action behaviortree_cpp std_srvs + libssh-dev ament_lint_auto ament_lint_common diff --git a/panther_manager/src/plugins/call_set_bool_service_node.cpp b/panther_manager/src/plugins/action/call_set_bool_service_node.cpp similarity index 92% rename from panther_manager/src/plugins/call_set_bool_service_node.cpp rename to panther_manager/src/plugins/action/call_set_bool_service_node.cpp index cd51f8e37..70ee082f2 100644 --- a/panther_manager/src/plugins/call_set_bool_service_node.cpp +++ b/panther_manager/src/plugins/action/call_set_bool_service_node.cpp @@ -1,4 +1,4 @@ -#include +#include namespace panther_manager { diff --git a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp b/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp similarity index 94% rename from panther_manager/src/plugins/call_set_led_animation_service_node.cpp rename to panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp index c3f205270..487af8a49 100644 --- a/panther_manager/src/plugins/call_set_led_animation_service_node.cpp +++ b/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp @@ -1,4 +1,4 @@ -#include +#include namespace panther_manager { diff --git a/panther_manager/src/plugins/call_trigger_service_node.cpp b/panther_manager/src/plugins/action/call_trigger_service_node.cpp similarity index 91% rename from panther_manager/src/plugins/call_trigger_service_node.cpp rename to panther_manager/src/plugins/action/call_trigger_service_node.cpp index 2206d55db..a09c1023d 100644 --- a/panther_manager/src/plugins/call_trigger_service_node.cpp +++ b/panther_manager/src/plugins/action/call_trigger_service_node.cpp @@ -1,4 +1,4 @@ -#include +#include namespace panther_manager { diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp new file mode 100644 index 000000000..810f6392b --- /dev/null +++ b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp @@ -0,0 +1,68 @@ +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace panther_manager +{ + +void ShutdownHostsFromFile::update_hosts(std::vector> & hosts) +{ + std::string shutdown_hosts_file; + if ( + !getInput("shutdown_hosts_file", shutdown_hosts_file) || + shutdown_hosts_file == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [shutdown_hosts_file]")); + } + + YAML::Node shutdown_hosts; + try { + shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); + } catch (const YAML::Exception & e) { + throw BT::RuntimeError("[" + name() + "] Error loading YAML file: " + e.what()); + } + + for (const auto & host : shutdown_hosts["hosts"]) { + if (!host["ip"] || !host["username"]) { + ROS_ERROR("[%s] Missing info for remote host!", get_node_name().c_str()); + continue; + } + + auto ip = host["ip"].as(); + auto user = host["username"].as(); + unsigned port = 22; + if (host["port"]) { + port = host["port"].as(); + } + std::string command = "sudo shutdown now"; + if (host["command"]) { + command = host["command"].as(); + } + float timeout = 5.0; + if (host["timeout"]) { + timeout = host["timeout"].as(); + } + bool ping_for_success = true; + if (host["ping_for_success"]) { + ping_for_success = host["ping_for_success"].as(); + } + + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); + } +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ShutdownHostsFromFile"); +} \ No newline at end of file diff --git a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp b/panther_manager/src/plugins/action/shutdown_single_host_node.cpp new file mode 100644 index 000000000..0bdfdc6ac --- /dev/null +++ b/panther_manager/src/plugins/action/shutdown_single_host_node.cpp @@ -0,0 +1,57 @@ +#include + +#include +#include +#include + +#include +#include + +#include + +namespace panther_manager +{ + +void ShutdownSingleHost::update_hosts(std::vector> & hosts) +{ + std::string ip; + if (!getInput("ip", ip) || ip == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [ip]")); + } + + std::string user; + if (!getInput("user", user) || user == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [user]")); + } + + unsigned port; + if (!getInput("port", port)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [port]")); + } + + std::string command; + if (!getInput("command", command) || command == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [command]")); + } + + float timeout; + if (!getInput("timeout", timeout)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [timeout]")); + } + + bool ping_for_success; + if (!getInput("ping_for_success", ping_for_success)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [ping_for_success]")); + } + + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ShutdownSingleHost"); +} \ No newline at end of file diff --git a/panther_manager/src/plugins/action/signal_shutdown_node.cpp b/panther_manager/src/plugins/action/signal_shutdown_node.cpp new file mode 100644 index 000000000..2968581e6 --- /dev/null +++ b/panther_manager/src/plugins/action/signal_shutdown_node.cpp @@ -0,0 +1,30 @@ +#include + +#include +#include + +#include +#include + +namespace panther_manager +{ + +BT::NodeStatus SignalShutdown::tick() +{ + auto reason = getInput("reason").value(); + + std::pair signal_shutdown; + signal_shutdown.first = true; + signal_shutdown.second = reason; + config().blackboard->set>("signal_shutdown", signal_shutdown); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SignalShutdown"); +} \ No newline at end of file diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index eb839ebb2..376f43b25 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_set_bool_plugin.cpp index 98a892ff9..bff044cdd 100644 --- a/panther_manager/test/plugins/test_set_bool_plugin.cpp +++ b/panther_manager/test/plugins/test_set_bool_plugin.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp index 8f5db5acf..a28c9c0cf 100644 --- a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp +++ b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_trigger_plugin.cpp index 7e215b7be..6cbb3b87a 100644 --- a/panther_manager/test/plugins/test_trigger_plugin.cpp +++ b/panther_manager/test/plugins/test_trigger_plugin.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -40,8 +40,6 @@ void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr req RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } - - TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) { std::vector services = { "test_trigger_service"}; From d84870b76cc98c727aa3b9bd072c31c846894395 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Feb 2024 10:42:48 +0000 Subject: [PATCH 10/35] Removed different types of building behavio tree Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 3 + .../panther_manager_plugin_test_utils.hpp | 16 +-- .../test/plugins/signal_shutdown_plugin.cpp | 125 ++++++++++++++++++ .../src/panther_manager_plugin_test_utils.cpp | 65 +-------- .../test/plugins/test_set_bool_plugin.cpp | 28 ++-- .../plugins/test_set_led_animation_plugin.cpp | 65 ++++----- .../test/plugins/test_trigger_plugin.cpp | 2 + 7 files changed, 193 insertions(+), 111 deletions(-) create mode 100644 panther_manager/test/plugins/signal_shutdown_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 28fc000b1..22b480fca 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -31,6 +31,9 @@ list(APPEND plugin_libs call_trigger_service_bt_node) add_library(call_set_led_animation_service_bt_node SHARED src/plugins/action/call_set_led_animation_service_node.cpp) list(APPEND plugin_libs call_set_led_animation_service_bt_node) +add_library(signal_shutdown_bt_node SHARED src/plugins/action/signal_shutdown_node.cpp) +list(APPEND plugin_libs signal_shutdown_bt_node) + foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 376f43b25..2c862278c 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -32,21 +32,21 @@ namespace panther_manager_plugin_test { -typedef std::array SetLEDAnimationTestUtils; +struct BehaviorTreePluginDescription{ + std::string service_name; + std::map params; +}; class PantherManagerPluginTestUtils { public: + std::string BuildBehaviorTree(const std::string& plugin_name, - const std::map& name_and_data_map); - std::string BuildBehaviorTree(const std::string& plugin_name, const std::vector& names); - std::string BuildBehaviorTree(const std::string& plugin_name, - const std::map& animation_params); + const BehaviorTreePluginDescription& service); - BT::Tree& CreateTree(const std::string& plugin_name, const std::map& name_and_data_map); - BT::Tree& CreateTree(const std::string& plugin_name, const std::vector& names); BT::Tree& CreateTree(const std::string& plugin_name, - const std::map& animation_params); + const BehaviorTreePluginDescription& service); + BT::BehaviorTreeFactory& GetFactory(); void Start(); diff --git a/panther_manager/test/plugins/signal_shutdown_plugin.cpp b/panther_manager/test/plugins/signal_shutdown_plugin.cpp new file mode 100644 index 000000000..de579d162 --- /dev/null +++ b/panther_manager/test/plugins/signal_shutdown_plugin.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +#include +#include +#include + +#include + +#include + +void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + [[maybe_unused]] request; + response->message = "Failed callback pass!"; + response->success = false; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); +} + +void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + [[maybe_unused]] request; + response->message = "Successfully callback pass!"; + response->success = true; + RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); +} + +TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) +{ + panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + + ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); + test_utils.Stop(); +} + +TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) +{ + panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) +{ + panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Found trigger service but shouldn't!"; + } + + test_utils.Stop(); +} + +TEST(TestCallTriggerService, good_trigger_call_service_success) +{ + panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", service); + + test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS) + { + FAIL() << "Cannot call trigger service!"; + } + + test_utils.Stop(); +} + +TEST(TestCallTriggerService, wrong_trigger_call_service_failure) +{ + panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("CallTriggerService", service); + + test_utils.CreateTriggerServiceServer(ServiceFailedCallback); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::FAILURE) + { + FAIL() << "Cannot call trigger service!"; + } + + test_utils.Stop(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index 3b6678410..c58b714e6 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -4,78 +4,27 @@ namespace panther_manager_plugin_test { std::string PantherManagerPluginTestUtils::BuildBehaviorTree( - const std::string& plugin_name, const std::map& name_and_data_map) + const std::string& plugin_name, const BehaviorTreePluginDescription& service) { std::stringstream bt; bt << header_ << std::endl; + bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << service.service_name <<"\" "; - for (auto const& [name, value] : name_and_data_map) + for (auto const& [key, value] : service.params) { - bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" data=\"" << value - << "\" />" << std::endl; + bt << key << "=\"" << value << "\" "; } - bt << footer_; + bt << " />" << std::endl << footer_; return bt.str(); } -std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& plugin_name, - const std::vector& names) -{ - std::stringstream bt; - - bt << header_ << std::endl; - - for (auto const& name : names) - { - bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" />" << std::endl; - } - - bt << footer_; - - return bt.str(); -} - -std::string PantherManagerPluginTestUtils::BuildBehaviorTree( - const std::string& plugin_name, const std::map& animation_params) -{ - std::stringstream bt; - - bt << header_ << std::endl; - - for (auto const& [name, param] : animation_params) - { - bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << name << "\" id=\"" << param[0] << "\" param=\"" - << param[1] << "\" repeating=\"" << param[2] << "\" />" << std::endl; - } - - bt << footer_; - - return bt.str(); -} - -BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, - const std::map& name_and_data_map) -{ - auto xml_text = BuildBehaviorTree(plugin_name, name_and_data_map); - tree_ = factory_.createTreeFromText(xml_text); - return tree_; -} - -BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, - const std::vector& names) -{ - auto xml_text = BuildBehaviorTree(plugin_name, names); - tree_ = factory_.createTreeFromText(xml_text); - return tree_; -} - BT::Tree& PantherManagerPluginTestUtils::CreateTree( - const std::string& plugin_name, const std::map& animation_params) + const std::string& plugin_name, const BehaviorTreePluginDescription& service) { - auto xml_text = BuildBehaviorTree(plugin_name, animation_params); + auto xml_text = BuildBehaviorTree(plugin_name, service); tree_ = factory_.createTreeFromText(xml_text); return tree_; } diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_set_bool_plugin.cpp index bff044cdd..223bc5b24 100644 --- a/panther_manager/test/plugins/test_set_bool_plugin.cpp +++ b/panther_manager/test/plugins/test_set_bool_plugin.cpp @@ -53,32 +53,32 @@ void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) { - std::map services = { { "test_set_bool_service", "true" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", services); }); + ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", service); }); test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) { - std::map services = { { "test_set_bool_service", "true" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", services); }, BT::RuntimeError); + EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) { - std::map services = { { "set_bool", "true" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", services); + auto& tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) @@ -91,11 +91,11 @@ TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_init TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) { - std::map services = { { "set_bool", "true" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", services); + auto& tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); @@ -110,11 +110,11 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) { - std::map services = { { "set_bool", "false" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "false" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", services); + auto& tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); @@ -129,11 +129,11 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) { - std::map services = { { "set_bool", "false" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "false" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", services); + auto& tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); @@ -148,11 +148,11 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) TEST(TestCallSetBoolService, wrong_service_value_defined) { - std::map services = { { "set_bool", "wrong_bool" } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "wrong_bool" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", services); + auto& tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) diff --git a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp index a28c9c0cf..e446584c9 100644 --- a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp +++ b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp @@ -35,7 +35,7 @@ void ServiceFailedCallback(const panther_msgs::srv::SetLEDAnimation::Request::Sh } void ServiceSuccessCallbackCheckRepeatingTrueValue(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; @@ -47,8 +47,9 @@ void ServiceSuccessCallbackCheckRepeatingTrueValue(const panther_msgs::srv::SetL EXPECT_EQ(request->repeating, true); } -void ServiceSuccessCallbackCheckRepeatingFalseValue(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +void ServiceSuccessCallbackCheckRepeatingFalseValue( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; @@ -61,7 +62,7 @@ void ServiceSuccessCallbackCheckRepeatingFalseValue(const panther_msgs::srv::Set } void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; @@ -75,34 +76,34 @@ void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Re TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service_plugin) { - std::map services = { { "set_led_animation", - { "0", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", services); }); + ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", service); }); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) { - std::map services = { { "set_led_animation", - { "0", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongCallSetLedAnimationService", services); }, BT::RuntimeError); + EXPECT_THROW({ test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_service_server_not_initialized) { - std::map services = { { "set_led_animation", - { "0", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) @@ -115,12 +116,12 @@ TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_servic TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_true_repeating_value) { - std::map services = { { "set_led_animation", - { "0", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); @@ -135,12 +136,12 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_false_repeating_value) { - std::map services = { { "set_led_animation", - { "0", "", "false" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "false" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); @@ -155,12 +156,12 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) { - std::map services = { { "set_led_animation", - { "5", "", "false" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "5" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); @@ -175,12 +176,12 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) { - std::map services = { { "set_led_animation", - { "0", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); @@ -195,11 +196,12 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) { - std::map services = { { "set_led_animation", - { "0", "", "wrong_bool" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "0" }, { "param", "" }, { "repeating", "wrong_bool" } } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) @@ -212,11 +214,12 @@ TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) { - std::map services = { { "set_led_animation", - { "-5", "", "true" } } }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { + { "id", "-5" }, { "param", "" }, { "repeating", "true" } } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", services); + auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_trigger_plugin.cpp index 6cbb3b87a..cc0c95c21 100644 --- a/panther_manager/test/plugins/test_trigger_plugin.cpp +++ b/panther_manager/test/plugins/test_trigger_plugin.cpp @@ -27,6 +27,7 @@ void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { + [[maybe_unused]] request; response->message = "Failed callback pass!"; response->success = false; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); @@ -35,6 +36,7 @@ void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr requ void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response) { + [[maybe_unused]] request; response->message = "Successfully callback pass!"; response->success = true; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); From 4da02fadf6858d71c8958652a40cd46ddd8e4858 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Feb 2024 10:47:23 +0000 Subject: [PATCH 11/35] Fixed building trigger Signed-off-by: Jakub Delicat --- .../test/plugins/test_trigger_plugin.cpp | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_trigger_plugin.cpp index cc0c95c21..220ddfe2e 100644 --- a/panther_manager/test/plugins/test_trigger_plugin.cpp +++ b/panther_manager/test/plugins/test_trigger_plugin.cpp @@ -34,7 +34,7 @@ void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr requ } void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) + std_srvs::srv::Trigger::Response::SharedPtr response) { [[maybe_unused]] request; response->message = "Successfully callback pass!"; @@ -44,32 +44,32 @@ void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr req TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) { - std::vector services = { "test_trigger_service"}; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", services); }); + ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); test_utils.Stop(); } TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) { - std::vector services = {"test_trigger_service" }; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", services); }, BT::RuntimeError); + EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); test_utils.Stop(); } TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) { - std::vector services = { "trigger"}; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", services); + auto& tree = test_utils.CreateTree("CallTriggerService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); if (status != BT::NodeStatus::FAILURE) @@ -82,11 +82,11 @@ TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initi TEST(TestCallTriggerService, good_trigger_call_service_success) { - std::vector services = { "trigger"}; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", services); + auto& tree = test_utils.CreateTree("CallTriggerService", service); test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); @@ -99,14 +99,13 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) test_utils.Stop(); } - TEST(TestCallTriggerService, wrong_trigger_call_service_failure) { - std::vector services = {"trigger"}; + panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", services); + auto& tree = test_utils.CreateTree("CallTriggerService", service); test_utils.CreateTriggerServiceServer(ServiceFailedCallback); @@ -119,7 +118,6 @@ TEST(TestCallTriggerService, wrong_trigger_call_service_failure) test_utils.Stop(); } - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 85127455ab1c6619b6e3b1882a2e3853640028c8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Feb 2024 12:25:04 +0000 Subject: [PATCH 12/35] Added tests for signal shutdown plugin Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 7 + .../panther_manager_plugin_test_utils.hpp | 5 +- .../test/plugins/signal_shutdown_plugin.cpp | 125 ------------------ .../src/panther_manager_plugin_test_utils.cpp | 24 ++-- .../test/plugins/test_set_bool_plugin.cpp | 39 ++---- .../plugins/test_set_led_animation_plugin.cpp | 80 +++++------ .../plugins/test_signal_shutdown_plugin.cpp | 87 ++++++++++++ .../test/plugins/test_trigger_plugin.cpp | 27 ++-- 8 files changed, 166 insertions(+), 228 deletions(-) delete mode 100644 panther_manager/test/plugins/signal_shutdown_plugin.cpp create mode 100644 panther_manager/test/plugins/test_signal_shutdown_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 22b480fca..b8f4bba5a 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -50,6 +50,7 @@ if(BUILD_TESTING) call_set_bool_service_bt_node call_trigger_service_bt_node call_set_led_animation_service_bt_node + signal_shutdown_bt_node ) ament_target_dependencies( @@ -74,6 +75,12 @@ if(BUILD_TESTING) ) list(APPEND plugin_tests ${PROJECT_NAME}_test_set_led_animation_plugin) + ament_add_gtest( + ${PROJECT_NAME}_test_signal_shutdown_plugin + test/plugins/test_signal_shutdown_plugin.cpp + ) + list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_plugin) + foreach(bt_plugin_test ${plugin_tests}) target_link_libraries(${bt_plugin_test} ${PROJECT_NAME}_test_plugin_utils) ament_target_dependencies(${bt_plugin_test} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 2c862278c..ae2f15613 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include @@ -42,10 +43,10 @@ class PantherManagerPluginTestUtils public: std::string BuildBehaviorTree(const std::string& plugin_name, - const BehaviorTreePluginDescription& service); + const std::map & service); BT::Tree& CreateTree(const std::string& plugin_name, - const BehaviorTreePluginDescription& service); + const std::map & service); BT::BehaviorTreeFactory& GetFactory(); diff --git a/panther_manager/test/plugins/signal_shutdown_plugin.cpp b/panther_manager/test/plugins/signal_shutdown_plugin.cpp deleted file mode 100644 index de579d162..000000000 --- a/panther_manager/test/plugins/signal_shutdown_plugin.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// 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 - -#include -#include -#include - -#include - -#include - -void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - [[maybe_unused]] request; - response->message = "Failed callback pass!"; - response->success = false; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); -} - -void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - [[maybe_unused]] request; - response->message = "Successfully callback pass!"; - response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); -} - -TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) -{ - panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - - ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); - test_utils.Stop(); -} - -TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) -{ - panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; - - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); - test_utils.Stop(); -} - -TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) -{ - panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; - - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Found trigger service but shouldn't!"; - } - - test_utils.Stop(); -} - -TEST(TestCallTriggerService, good_trigger_call_service_success) -{ - panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; - - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); - - test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call trigger service!"; - } - - test_utils.Stop(); -} - -TEST(TestCallTriggerService, wrong_trigger_call_service_failure) -{ - panther_manager_plugin_test::BehaviorTreePluginDescription service = { { "test_trigger_service" }, {} }; - - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); - - test_utils.CreateTriggerServiceServer(ServiceFailedCallback); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Cannot call trigger service!"; - } - - test_utils.Stop(); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index c58b714e6..4a6a16ff1 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -3,28 +3,30 @@ namespace panther_manager_plugin_test { -std::string PantherManagerPluginTestUtils::BuildBehaviorTree( - const std::string& plugin_name, const BehaviorTreePluginDescription& service) +std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& plugin_name, + const std::map & service) { std::stringstream bt; bt << header_ << std::endl; - bt << "\t\t\t\t<" << plugin_name << " service_name=\"" << service.service_name <<"\" "; + bt << "\t\t\t\t<" << plugin_name << " "; - for (auto const& [key, value] : service.params) + + for (auto const& [key, value] : service) { - bt << key << "=\"" << value << "\" "; + bt << key << "=\"" << value << "\" "; } - bt << " />" << std::endl << footer_; + bt << " />" << std::endl << footer_; return bt.str(); } -BT::Tree& PantherManagerPluginTestUtils::CreateTree( - const std::string& plugin_name, const BehaviorTreePluginDescription& service) +BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, + const std::map & service) { auto xml_text = BuildBehaviorTree(plugin_name, service); + std::cout << xml_text; tree_ = factory_.createTreeFromText(xml_text); return tree_; } @@ -44,6 +46,7 @@ void PantherManagerPluginTestUtils::Start() factory_.registerNodeType("CallSetBoolService", params); factory_.registerNodeType("CallTriggerService", params); factory_.registerNodeType("CallSetLedAnimationService", params); + factory_.registerNodeType("SignalShutdown"); } void PantherManagerPluginTestUtils::Stop() @@ -84,8 +87,9 @@ void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( panther_msgs::srv::SetLEDAnimation::Response::SharedPtr)> service_callback) { - service_server_node_ = std::make_shared("test_set_led_animation_service"); - set_led_animation_server_ = service_server_node_->create_service("set_led_animation", service_callback); + service_server_node_ = std::make_shared("test_set_led_animation_service"); + set_led_animation_server_ = + service_server_node_->create_service("set_led_animation", service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_set_bool_plugin.cpp index 223bc5b24..fd6ef05e5 100644 --- a/panther_manager/test/plugins/test_set_bool_plugin.cpp +++ b/panther_manager/test/plugins/test_set_bool_plugin.cpp @@ -53,7 +53,7 @@ void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -64,7 +64,7 @@ TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -74,24 +74,21 @@ TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plu TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Found set_bool service but shouldn't!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "true" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -100,17 +97,14 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call set_bool service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "false" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "false" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -119,17 +113,14 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call set_bool service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "false" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "false" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -138,27 +129,21 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Cannot call set_bool service!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_service_value_defined) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_bool", { { "data", "wrong_bool" } } }; + std::map service = { { "service_name", "set_bool" }, { "data", "wrong_bool" } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Wrong value is parsed as good value!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } diff --git a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp index e446584c9..881811bd1 100644 --- a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp +++ b/panther_manager/test/plugins/test_set_led_animation_plugin.cpp @@ -76,8 +76,9 @@ void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Re TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -87,8 +88,9 @@ TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service TEST(TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -98,26 +100,25 @@ TEST(TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_anim TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_service_server_not_initialized) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Found set_led_animation service but shouldn't!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_true_repeating_value) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -126,18 +127,16 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call set_led_animation service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_false_repeating_value) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "false" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "false" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -146,18 +145,16 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call set_led_animation service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "5" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "5" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -166,18 +163,16 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call set_led_animation service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -186,46 +181,39 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Cannot call set_led_animation service!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "0" }, { "param", "" }, { "repeating", "wrong_bool" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "wrong_bool" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Wrong value is parsed as good value!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "set_led_animation", { - { "id", "-5" }, { "param", "" }, { "repeating", "true" } } }; + std::map service = { + { "service_name", "set_led_animation" }, { "id", "-5" }, { "param", "" }, { "repeating", "true" } + }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Wrong value is parsed as good value!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } diff --git a/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp b/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp new file mode 100644 index 000000000..f6db2271c --- /dev/null +++ b/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp @@ -0,0 +1,87 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +#include +#include +#include + +#include +#include + +TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) +{ + std::map service = { { "reason", "Test shutdown." } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("SignalShutdown", service); + + ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); + test_utils.Stop(); +} + +TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) +{ + std::map service = {}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongSignalShutdown", service); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestSignalShutdown, good_check_blackboard_value) +{ + std::map service = { { "reason", "Test shutdown." } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("SignalShutdown", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + + auto blackboard = tree.rootBlackboard(); + auto got_value = blackboard->get>("signal_shutdown"); + + EXPECT_EQ(got_value.first, true); + EXPECT_EQ(got_value.second, service["reason"]); + test_utils.Stop(); +} + +TEST(TestSignalShutdown, wrong_check_blackboard_value) +{ + std::map service = { { "reason", "Test shutdown." } }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto& tree = test_utils.CreateTree("SignalShutdown", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + + auto blackboard = tree.rootBlackboard(); + auto got_value = blackboard->get>("signal_shutdown"); + + EXPECT_EQ(got_value.first, true); + EXPECT_FALSE(got_value.second == "Wrong reason!"); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_trigger_plugin.cpp index 220ddfe2e..7d94a9164 100644 --- a/panther_manager/test/plugins/test_trigger_plugin.cpp +++ b/panther_manager/test/plugins/test_trigger_plugin.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr req TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; + std::map service = { { "service_name", "trigger" }}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -55,7 +55,7 @@ TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; + std::map service = { { "service_name", "trigger" }}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -65,24 +65,21 @@ TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plug TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; + std::map service = { { "service_name", "trigger" }}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); auto& tree = test_utils.CreateTree("CallTriggerService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Found trigger service but shouldn't!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } TEST(TestCallTriggerService, good_trigger_call_service_success) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; + std::map service = { { "service_name", "trigger" }}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -91,17 +88,14 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS) - { - FAIL() << "Cannot call trigger service!"; - } + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); test_utils.Stop(); } TEST(TestCallTriggerService, wrong_trigger_call_service_failure) { - panther_manager_plugin_test::BehaviorTreePluginDescription service = { "trigger", {} }; + std::map service = { { "service_name", "trigger" }}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -110,10 +104,7 @@ TEST(TestCallTriggerService, wrong_trigger_call_service_failure) test_utils.CreateTriggerServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::FAILURE) - { - FAIL() << "Cannot call trigger service!"; - } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); } From 93995d210ba8a03e2e93b7fdae4f809a31158f9d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Feb 2024 08:33:08 +0000 Subject: [PATCH 13/35] added test single plugin Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 12 +++ .../action/shutdown_single_host_node.hpp | 2 +- .../panther_manager/plugins/shutdown_host.hpp | 12 +-- .../{action => }/shutdown_hosts_node.hpp | 21 ++--- panther_manager/package.xml | 1 + .../action/shutdown_hosts_from_file_node.cpp | 40 +++++---- .../panther_manager_plugin_test_utils.hpp | 1 + .../src/panther_manager_plugin_test_utils.cpp | 1 + .../test_shutdown_single_host_plugin.cpp | 84 +++++++++++++++++++ .../plugins/test_signal_shutdown_plugin.cpp | 4 +- 10 files changed, 143 insertions(+), 35 deletions(-) rename panther_manager/include/panther_manager/plugins/{action => }/shutdown_hosts_node.hpp (88%) create mode 100644 panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index b8f4bba5a..1fb889599 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -13,6 +13,7 @@ set(PACKAGE_INCLUDE_DEPENDS behaviortree_cpp std_srvs panther_msgs + libssh ) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) @@ -34,6 +35,10 @@ list(APPEND plugin_libs call_set_led_animation_service_bt_node) add_library(signal_shutdown_bt_node SHARED src/plugins/action/signal_shutdown_node.cpp) list(APPEND plugin_libs signal_shutdown_bt_node) +add_library(shutdown_single_host_bt_node SHARED src/plugins/action/shutdown_single_host_node.cpp) +target_link_libraries(shutdown_single_host_bt_node ssh) +list(APPEND plugin_libs shutdown_single_host_bt_node) + foreach(bt_plugin ${plugin_libs}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) @@ -51,6 +56,7 @@ if(BUILD_TESTING) call_trigger_service_bt_node call_set_led_animation_service_bt_node signal_shutdown_bt_node + shutdown_single_host_bt_node ) ament_target_dependencies( @@ -81,6 +87,12 @@ if(BUILD_TESTING) ) list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_plugin) + ament_add_gtest( + ${PROJECT_NAME}_test_shutdown_single_host_plugin + test/plugins/test_shutdown_single_host_plugin.cpp + ) + list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_plugin) + foreach(bt_plugin_test ${plugin_tests}) target_link_libraries(${bt_plugin_test} ${PROJECT_NAME}_test_plugin_utils) ament_target_dependencies(${bt_plugin_test} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 1b0cfdc6b..01880dd04 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -44,7 +44,7 @@ class ShutdownSingleHost : public ShutdownHosts BT::InputPort("command", "command to execute on shutdown"), BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), BT::InputPort( - "ping_for_success", "ping host unitl it is not available or timeout is reached"), + "ping_for_success", "ping host until it is not available or timeout is reached"), }; } diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index ad11f12ab..382a26fba 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -86,7 +86,7 @@ class ShutdownHost { request_shutdown(); } - catch (std::runtime_error err) + catch (const std::runtime_error &err) { state_ = ShutdownHostState::FAILURE; failure_reason_ = err.what(); @@ -103,7 +103,7 @@ class ShutdownHost break; } } - catch (std::runtime_error err) + catch (const std::runtime_error &err) { state_ = ShutdownHostState::FAILURE; failure_reason_ = err.what(); @@ -205,11 +205,12 @@ class ShutdownHost private: const std::string ip_; const std::string user_; - const std::string command_; - const std::size_t hash_; const int port_; - const bool ping_for_success_; + const std::string command_; const std::chrono::milliseconds timeout_; + const bool ping_for_success_; + const std::size_t hash_; + ShutdownHostState state_; char buffer_[1024]; const int verbosity_ = SSH_LOG_NOLOG; @@ -217,7 +218,6 @@ class ShutdownHost std::string output_; std::string failure_reason_; std::chrono::time_point command_time_; - ShutdownHostState state_; ssh_session session_; ssh_channel channel_; diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp similarity index 88% rename from panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp rename to panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 386094007..7c9c5fd7e 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -42,7 +42,7 @@ class ShutdownHosts : public BT::StatefulActionNode { // TODO: @delihus What is the name of ros::this_node? node_name_ = name; - logger_ = std::make_shared({ rclcpp::get_logger("node_name_") }); + logger_ = std::make_shared(rclcpp::get_logger(node_name_)); } virtual ~ShutdownHosts() = default; @@ -71,14 +71,14 @@ class ShutdownHosts : public BT::StatefulActionNode } private: - int check_host_index_ = 0; + std::size_t check_host_index_ = 0; std::string node_name_; std::vector> hosts_; std::vector hosts_to_check_; std::vector skipped_hosts_; std::vector succeeded_hosts_; std::vector failed_hosts_; - rclcpp::Logger::SharedPtr logger_; + std::shared_ptr logger_; BT::NodeStatus onStart() { @@ -86,7 +86,7 @@ class ShutdownHosts : public BT::StatefulActionNode remove_duplicate_hosts(hosts_); if (hosts_.size() <= 0) { - RCLCPP_ERROR_STREAM(&logger_, "Hosts list is empty! Check configuration!"); + RCLCPP_ERROR_STREAM(*logger_, "Hosts list is empty! Check configuration!"); return BT::NodeStatus::FAILURE; } hosts_to_check_.resize(hosts_.size()); @@ -113,26 +113,27 @@ class ShutdownHosts : public BT::StatefulActionNode switch (host->get_state()) { case ShutdownHostState::RESPONSE_RECEIVED: - RCLCPP_INFO_STREAM(&logger_, "Device at: " << host->get_ip() << " response:\n" << host->get_response()); + RCLCPP_INFO_STREAM(*logger_, "Device at: " << host->get_ip() << " response:\n" << host->get_response()); check_host_index_++; break; case ShutdownHostState::SUCCESS: - RCLCPP_INFO_STREAM(&logger_, "Successfully shutdown device at: " << host->get_ip()); + RCLCPP_INFO_STREAM(*logger_, "Successfully shutdown device at: " << host->get_ip()); succeeded_hosts_.push_back(host_index); hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); break; case ShutdownHostState::FAILURE: - ROS_WARN("[%s] Failed to shutdown device at: %s. Error: %s", node_name_.c_str(), host->get_ip().c_str(), - host->get_error().c_str()); + RCLCPP_WARN_STREAM(*logger_, + "Failed to shutdown device at: " << host->get_ip() << " Error: " << host->get_error()); + failed_hosts_.push_back(host_index); hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); break; case ShutdownHostState::SKIPPED: - RCLCPP_WARN_STREAM(&logger_, "Device at: " << host->get_ip() << " not available, skipping..."); + RCLCPP_WARN_STREAM(*logger_, "Device at: " << host->get_ip() << " not available, skipping..."); skipped_hosts_.push_back(host_index); hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); @@ -159,7 +160,7 @@ class ShutdownHosts : public BT::StatefulActionNode } else { - RCLCPP_WARN_STREAM(&logger_, "Found duplicate host: " << host->get_ip() + RCLCPP_WARN_STREAM(*logger_, "Found duplicate host: " << host->get_ip() << " Processing only the " "first " "occurrence."); diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 4030b1f50..38f9ff266 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -20,6 +20,7 @@ behaviortree_cpp std_srvs libssh-dev + iputils-ping ament_lint_auto ament_lint_common diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp index 810f6392b..cb14f2b19 100644 --- a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp @@ -7,55 +7,63 @@ #include #include #include +#include #include namespace panther_manager { -void ShutdownHostsFromFile::update_hosts(std::vector> & hosts) +void ShutdownHostsFromFile::update_hosts(std::vector>& hosts) { std::string shutdown_hosts_file; - if ( - !getInput("shutdown_hosts_file", shutdown_hosts_file) || - shutdown_hosts_file == "") { + if (!getInput("shutdown_hosts_file", shutdown_hosts_file) || shutdown_hosts_file == "") + { throw(BT::RuntimeError("[", name(), "] Failed to get input [shutdown_hosts_file]")); } YAML::Node shutdown_hosts; - try { + try + { shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); - } catch (const YAML::Exception & e) { + } + catch (const YAML::Exception& e) + { throw BT::RuntimeError("[" + name() + "] Error loading YAML file: " + e.what()); } - for (const auto & host : shutdown_hosts["hosts"]) { - if (!host["ip"] || !host["username"]) { - ROS_ERROR("[%s] Missing info for remote host!", get_node_name().c_str()); + for (const auto& host : shutdown_hosts["hosts"]) + { + if (!host["ip"] || !host["username"]) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(get_node_name()), "Missing info for remote host!"); continue; } auto ip = host["ip"].as(); auto user = host["username"].as(); unsigned port = 22; - if (host["port"]) { + if (host["port"]) + { port = host["port"].as(); } std::string command = "sudo shutdown now"; - if (host["command"]) { + if (host["command"]) + { command = host["command"].as(); } float timeout = 5.0; - if (host["timeout"]) { + if (host["timeout"]) + { timeout = host["timeout"].as(); } bool ping_for_success = true; - if (host["ping_for_success"]) { + if (host["ping_for_success"]) + { ping_for_success = host["ping_for_success"].as(); } - hosts.push_back( - std::make_shared(ip, user, port, command, timeout, ping_for_success)); + hosts.push_back(std::make_shared(ip, user, port, command, timeout, ping_for_success)); } } @@ -65,4 +73,4 @@ void ShutdownHostsFromFile::update_hosts(std::vector("ShutdownHostsFromFile"); -} \ No newline at end of file +} diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index ae2f15613..bd452b84e 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index 4a6a16ff1..8d397062c 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -47,6 +47,7 @@ void PantherManagerPluginTestUtils::Start() factory_.registerNodeType("CallTriggerService", params); factory_.registerNodeType("CallSetLedAnimationService", params); factory_.registerNodeType("SignalShutdown"); + factory_.registerNodeType("ShutdownSingleHost"); } void PantherManagerPluginTestUtils::Stop() diff --git a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp new file mode 100644 index 000000000..a57a519c5 --- /dev/null +++ b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include + +#include +#include +#include + +#include +#include + +TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) +{ + std::map service = { + { "command", "pwd" }, { "ip", "localhost" }, { "ping_for_success", "false" }, + { "port", "22" }, { "timeout", "5.0" }, { "user", "husarion" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + + ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownSingleHost", service); }); + test_utils.Stop(); +} + +TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plugin) +{ + std::map service = { + { "command", "pwd" }, { "ip", "localhost" }, { "ping_for_success", "false" }, + { "port", "22" }, { "timeout", "5.0" }, { "user", "husarion" }, + }; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestShutdownSingleHost, good_touch_command) +{ + std::string file_path = "/tmp/test_panther_manager_good_touch_command"; + std::filesystem::remove(file_path); + EXPECT_FALSE(std::filesystem::exists(file_path)); + + // #TODO: @delihus change the user to husarion on the panther + std::map service = { + { "command", "touch " + file_path }, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, + { "timeout", "5.0" }, { "user", "deli" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + + testing::internal::CaptureStdout(); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + EXPECT_TRUE(std::filesystem::exists(file_path)); + + std::filesystem::remove(file_path); + test_utils.Stop(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp b/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp index f6db2271c..294efe113 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp @@ -44,7 +44,7 @@ TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) test_utils.Stop(); } -TEST(TestSignalShutdown, good_check_blackboard_value) +TEST(TestSignalShutdown, good_check_reason_blackboard_value) { std::map service = { { "reason", "Test shutdown." } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; @@ -62,7 +62,7 @@ TEST(TestSignalShutdown, good_check_blackboard_value) test_utils.Stop(); } -TEST(TestSignalShutdown, wrong_check_blackboard_value) +TEST(TestSignalShutdown, wrong_check_reason_blackboard_value) { std::map service = { { "reason", "Test shutdown." } }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; From a4f9051c8dfcf03cefa4f827904126fb50c0b316 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Feb 2024 16:29:12 +0000 Subject: [PATCH 14/35] added test for single host plugin Signed-off-by: Jakub Delicat --- panther_gazebo/CMakeLists.txt | 8 - .../config/battery_plugin_config.yaml | 19 -- panther_gazebo/config/gz_bridge.yaml | 21 -- panther_gazebo/launch/simulation.launch.py | 242 ------------------ .../test_shutdown_single_host_plugin.cpp | 38 ++- 5 files changed, 35 insertions(+), 293 deletions(-) delete mode 100644 panther_gazebo/CMakeLists.txt delete mode 100644 panther_gazebo/config/battery_plugin_config.yaml delete mode 100644 panther_gazebo/config/gz_bridge.yaml delete mode 100644 panther_gazebo/launch/simulation.launch.py diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt deleted file mode 100644 index 0709e4501..000000000 --- a/panther_gazebo/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(panther_gazebo) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin_config.yaml deleted file mode 100644 index 665710fdc..000000000 --- a/panther_gazebo/config/battery_plugin_config.yaml +++ /dev/null @@ -1,19 +0,0 @@ -# Parameters for the Ignition LinearBatteryPlugin - -# Enables battery simulation. If set to true, the battery will discharge, -# and if it depletes completely, the robot will stop moving. -simulate_discharging: false - -# Sets the initial charge percentage of the battery. -initial_charge_percentage: 70.0 - -# Specifies the charging time for the battery in hours. -charging_time: 6.0 # [h] - -# Represents the power load during normal operation and is initially set to 120W. -# With the default power load of 120W, the robot can operate for up to 8 hours. -# When the 'simulate_discharging' parameter is set to false, this value defaults to 0. -# Users are encouraged to customize this value to match their specific needs. -# For more information on Panther power consumption, please refer to: -# https://husarion.com/manuals/panther/#battery--charging -power_load: 120.0 # [W] diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml deleted file mode 100644 index b66c67330..000000000 --- a/panther_gazebo/config/gz_bridge.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# For detailed information on configuring the parameter_bridge, please refer to: -# https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml - -- topic_name: "clock" - ros_type_name: "rosgraph_msgs/msg/Clock" - gz_type_name: "ignition.msgs.Clock" - direction: GZ_TO_ROS - -- ros_topic_name: "battery" - gz_topic_name: "model/panther/battery/panther_battery/state" - ros_type_name: "sensor_msgs/msg/BatteryState" - gz_type_name: "ignition.msgs.BatteryState" - subscriber_queue: 10 - publisher_queue: 10 - direction: GZ_TO_ROS - -# Topic for Twist data from Ignition teleopt widget -- topic_name: "cmd_vel" - ros_type_name: "geometry_msgs/msg/Twist" - gz_type_name: "ignition.msgs.Twist" - direction: GZ_TO_ROS diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py deleted file mode 100644 index 68bbe83c3..000000000 --- a/panther_gazebo/launch/simulation.launch.py +++ /dev/null @@ -1,242 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 Husarion sp. z o.o. -# -# 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. - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, - PythonExpression, -) -from launch_ros.actions import Node, SetParameter - - -def generate_launch_description(): - wheel_type = LaunchConfiguration("wheel_type") - declare_wheel_type_arg = DeclareLaunchArgument( - "wheel_type", - default_value="WH01", - description=( - "Specify the type of wheel. If you select a value from the provided options ('WH01'," - " 'WH02', 'WH04'), you can disregard the 'wheel_config_path' and" - " 'controller_config_path' parameters. If you have custom wheels, set this parameter" - " to 'CUSTOM' and provide the necessary configurations." - ), - choices=["WH01", "WH02", "WH04", "CUSTOM"], - ) - - wheel_config_path = LaunchConfiguration("wheel_config_path") - declare_wheel_config_path_arg = DeclareLaunchArgument( - "wheel_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_description"), - "config", - PythonExpression(["'", wheel_type, ".yaml'"]), - ] - ), - description=( - "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " - "to your custom wheel configuration file here. " - ), - ) - - controller_config_path = LaunchConfiguration("controller_config_path") - declare_controller_config_path_arg = DeclareLaunchArgument( - "controller_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_controller"), - "config", - PythonExpression(["'", wheel_type, "_controller.yaml'"]), - ] - ), - description=( - "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" - " the path to your custom controller configuration file here. " - ), - ) - - battery_config_path = LaunchConfiguration("battery_config_path") - declare_battery_config_path_arg = DeclareLaunchArgument( - "battery_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_gazebo"), - "config", - "battery_plugin_config.yaml", - ] - ), - description=( - "Path to the Ignition LinearBatteryPlugin configuration file. " - "This configuration is intended for use in simulations only." - ), - ) - - gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") - declare_gz_bridge_config_path_arg = DeclareLaunchArgument( - "gz_bridge_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_gazebo"), - "config", - "gz_bridge.yaml", - ] - ), - description="Path to the parameter_bridge configuration file", - ) - - world_cfg = LaunchConfiguration("world") - declare_world_arg = DeclareLaunchArgument( - "world", - default_value=[ - "-r ", - PathJoinSubstitution( - [ - get_package_share_directory("husarion_office_gz"), - "worlds", - "husarion_world.sdf", - ], - ), - ], - description="SDF world file", - ) - - pose_x = LaunchConfiguration("pose_x") - declare_pose_x_arg = DeclareLaunchArgument( - "pose_x", - default_value=["5.0"], - description="Initial robot position in the global 'x' axis.", - ) - - pose_y = LaunchConfiguration("pose_y") - declare_pose_y_arg = DeclareLaunchArgument( - "pose_y", - default_value=["-5.0"], - description="Initial robot position in the global 'y' axis.", - ) - - pose_z = LaunchConfiguration("pose_z") - declare_pose_z_arg = DeclareLaunchArgument( - "pose_z", - default_value=["0.2"], - description="Initial robot position in the global 'z' axis.", - ) - - rot_yaw = LaunchConfiguration("rot_yaw") - declare_rot_yaw_arg = DeclareLaunchArgument( - "rot_yaw", default_value=["0.0"], description="Initial robot orientation." - ) - - publish_robot_state = LaunchConfiguration("publish_robot_state") - declare_publish_robot_state_arg = DeclareLaunchArgument( - "publish_robot_state", - default_value="True", - description=( - "Whether to launch the robot_state_publisher node." - "When set to False, users should publish their own robot description." - ), - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={"gz_args": world_cfg}.items(), - ) - - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "panther", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - pose_x, - "-y", - pose_y, - "-z", - pose_z, - "-Y", - rot_yaw, - ], - output="screen", - ) - - gz_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_bridge", - parameters=[{"config_file": gz_bridge_config_path}], - output="screen", - ) - - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("panther_bringup"), - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "wheel_type": wheel_type, - "wheel_config_path": wheel_config_path, - "controller_config_path": controller_config_path, - "battery_config_path": battery_config_path, - "publish_robot_state": publish_robot_state, - "use_sim": "True", - "simulation_engine": "ignition-gazebo", - }.items(), - ) - - return LaunchDescription( - [ - declare_world_arg, - declare_pose_x_arg, - declare_pose_y_arg, - declare_pose_z_arg, - declare_rot_yaw_arg, - declare_wheel_type_arg, - declare_wheel_config_path_arg, - declare_controller_config_path_arg, - declare_battery_config_path_arg, - declare_gz_bridge_config_path_arg, - declare_publish_robot_state_arg, - # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - gz_bridge, - gz_spawn_entity, - bringup_launch, - ] - ) diff --git a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp index a57a519c5..f10a94c21 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp @@ -57,17 +57,15 @@ TEST(TestShutdownSingleHost, good_touch_command) std::filesystem::remove(file_path); EXPECT_FALSE(std::filesystem::exists(file_path)); - // #TODO: @delihus change the user to husarion on the panther std::map service = { { "command", "touch " + file_path }, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "deli" }, + { "timeout", "5.0" }, { "user", "husarion" }, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); - testing::internal::CaptureStdout(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); EXPECT_TRUE(std::filesystem::exists(file_path)); @@ -76,6 +74,40 @@ TEST(TestShutdownSingleHost, good_touch_command) test_utils.Stop(); } +TEST(TestShutdownSingleHost, wrong_command) +{ + std::map service = { + { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, + { "timeout", "5.0" }, { "user", "husarion" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + + test_utils.Stop(); +} + +TEST(TestShutdownSingleHost, wrong_user) +{ + std::map service = { + { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, + { "timeout", "5.0" }, { "user", "user_what_does_not_exists" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + + test_utils.Stop(); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 450b3bdb38c9d13fdd13ec5554932bc251ea7b0a Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Feb 2024 16:30:00 +0000 Subject: [PATCH 15/35] Revert "added test for single host plugin" This reverts commit a4f9051c8dfcf03cefa4f827904126fb50c0b316. --- panther_gazebo/CMakeLists.txt | 8 + .../config/battery_plugin_config.yaml | 19 ++ panther_gazebo/config/gz_bridge.yaml | 21 ++ panther_gazebo/launch/simulation.launch.py | 242 ++++++++++++++++++ .../test_shutdown_single_host_plugin.cpp | 38 +-- 5 files changed, 293 insertions(+), 35 deletions(-) create mode 100644 panther_gazebo/CMakeLists.txt create mode 100644 panther_gazebo/config/battery_plugin_config.yaml create mode 100644 panther_gazebo/config/gz_bridge.yaml create mode 100644 panther_gazebo/launch/simulation.launch.py diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt new file mode 100644 index 000000000..0709e4501 --- /dev/null +++ b/panther_gazebo/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.10.2) +project(panther_gazebo) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin_config.yaml new file mode 100644 index 000000000..665710fdc --- /dev/null +++ b/panther_gazebo/config/battery_plugin_config.yaml @@ -0,0 +1,19 @@ +# Parameters for the Ignition LinearBatteryPlugin + +# Enables battery simulation. If set to true, the battery will discharge, +# and if it depletes completely, the robot will stop moving. +simulate_discharging: false + +# Sets the initial charge percentage of the battery. +initial_charge_percentage: 70.0 + +# Specifies the charging time for the battery in hours. +charging_time: 6.0 # [h] + +# Represents the power load during normal operation and is initially set to 120W. +# With the default power load of 120W, the robot can operate for up to 8 hours. +# When the 'simulate_discharging' parameter is set to false, this value defaults to 0. +# Users are encouraged to customize this value to match their specific needs. +# For more information on Panther power consumption, please refer to: +# https://husarion.com/manuals/panther/#battery--charging +power_load: 120.0 # [W] diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml new file mode 100644 index 000000000..b66c67330 --- /dev/null +++ b/panther_gazebo/config/gz_bridge.yaml @@ -0,0 +1,21 @@ +# For detailed information on configuring the parameter_bridge, please refer to: +# https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml + +- topic_name: "clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "battery" + gz_topic_name: "model/panther/battery/panther_battery/state" + ros_type_name: "sensor_msgs/msg/BatteryState" + gz_type_name: "ignition.msgs.BatteryState" + subscriber_queue: 10 + publisher_queue: 10 + direction: GZ_TO_ROS + +# Topic for Twist data from Ignition teleopt widget +- topic_name: "cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "ignition.msgs.Twist" + direction: GZ_TO_ROS diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py new file mode 100644 index 000000000..68bbe83c3 --- /dev/null +++ b/panther_gazebo/launch/simulation.launch.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +# Copyright 2023 Husarion sp. z o.o. +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node, SetParameter + + +def generate_launch_description(): + wheel_type = LaunchConfiguration("wheel_type") + declare_wheel_type_arg = DeclareLaunchArgument( + "wheel_type", + default_value="WH01", + description=( + "Specify the type of wheel. If you select a value from the provided options ('WH01'," + " 'WH02', 'WH04'), you can disregard the 'wheel_config_path' and" + " 'controller_config_path' parameters. If you have custom wheels, set this parameter" + " to 'CUSTOM' and provide the necessary configurations." + ), + choices=["WH01", "WH02", "WH04", "CUSTOM"], + ) + + wheel_config_path = LaunchConfiguration("wheel_config_path") + declare_wheel_config_path_arg = DeclareLaunchArgument( + "wheel_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_description"), + "config", + PythonExpression(["'", wheel_type, ".yaml'"]), + ] + ), + description=( + "Path to wheel configuration file. By default, it is located in " + "'panther_description/config/.yaml'. You can also specify the path " + "to your custom wheel configuration file here. " + ), + ) + + controller_config_path = LaunchConfiguration("controller_config_path") + declare_controller_config_path_arg = DeclareLaunchArgument( + "controller_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_controller"), + "config", + PythonExpression(["'", wheel_type, "_controller.yaml'"]), + ] + ), + description=( + "Path to controller configuration file. By default, it is located in" + " 'panther_controller/config/_controller.yaml'. You can also specify" + " the path to your custom controller configuration file here. " + ), + ) + + battery_config_path = LaunchConfiguration("battery_config_path") + declare_battery_config_path_arg = DeclareLaunchArgument( + "battery_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_gazebo"), + "config", + "battery_plugin_config.yaml", + ] + ), + description=( + "Path to the Ignition LinearBatteryPlugin configuration file. " + "This configuration is intended for use in simulations only." + ), + ) + + gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") + declare_gz_bridge_config_path_arg = DeclareLaunchArgument( + "gz_bridge_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_gazebo"), + "config", + "gz_bridge.yaml", + ] + ), + description="Path to the parameter_bridge configuration file", + ) + + world_cfg = LaunchConfiguration("world") + declare_world_arg = DeclareLaunchArgument( + "world", + default_value=[ + "-r ", + PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "husarion_world.sdf", + ], + ), + ], + description="SDF world file", + ) + + pose_x = LaunchConfiguration("pose_x") + declare_pose_x_arg = DeclareLaunchArgument( + "pose_x", + default_value=["5.0"], + description="Initial robot position in the global 'x' axis.", + ) + + pose_y = LaunchConfiguration("pose_y") + declare_pose_y_arg = DeclareLaunchArgument( + "pose_y", + default_value=["-5.0"], + description="Initial robot position in the global 'y' axis.", + ) + + pose_z = LaunchConfiguration("pose_z") + declare_pose_z_arg = DeclareLaunchArgument( + "pose_z", + default_value=["0.2"], + description="Initial robot position in the global 'z' axis.", + ) + + rot_yaw = LaunchConfiguration("rot_yaw") + declare_rot_yaw_arg = DeclareLaunchArgument( + "rot_yaw", default_value=["0.0"], description="Initial robot orientation." + ) + + publish_robot_state = LaunchConfiguration("publish_robot_state") + declare_publish_robot_state_arg = DeclareLaunchArgument( + "publish_robot_state", + default_value="True", + description=( + "Whether to launch the robot_state_publisher node." + "When set to False, users should publish their own robot description." + ), + ) + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={"gz_args": world_cfg}.items(), + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "panther", + "-allow_renaming", + "true", + "-topic", + "robot_description", + "-x", + pose_x, + "-y", + pose_y, + "-z", + pose_z, + "-Y", + rot_yaw, + ], + output="screen", + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_bridge", + parameters=[{"config_file": gz_bridge_config_path}], + output="screen", + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("panther_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "wheel_type": wheel_type, + "wheel_config_path": wheel_config_path, + "controller_config_path": controller_config_path, + "battery_config_path": battery_config_path, + "publish_robot_state": publish_robot_state, + "use_sim": "True", + "simulation_engine": "ignition-gazebo", + }.items(), + ) + + return LaunchDescription( + [ + declare_world_arg, + declare_pose_x_arg, + declare_pose_y_arg, + declare_pose_z_arg, + declare_rot_yaw_arg, + declare_wheel_type_arg, + declare_wheel_config_path_arg, + declare_controller_config_path_arg, + declare_battery_config_path_arg, + declare_gz_bridge_config_path_arg, + declare_publish_robot_state_arg, + # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + gz_sim, + gz_bridge, + gz_spawn_entity, + bringup_launch, + ] + ) diff --git a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp index f10a94c21..a57a519c5 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp @@ -57,15 +57,17 @@ TEST(TestShutdownSingleHost, good_touch_command) std::filesystem::remove(file_path); EXPECT_FALSE(std::filesystem::exists(file_path)); + // #TODO: @delihus change the user to husarion on the panther std::map service = { { "command", "touch " + file_path }, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "husarion" }, + { "timeout", "5.0" }, { "user", "deli" }, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + testing::internal::CaptureStdout(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); EXPECT_TRUE(std::filesystem::exists(file_path)); @@ -74,40 +76,6 @@ TEST(TestShutdownSingleHost, good_touch_command) test_utils.Stop(); } -TEST(TestShutdownSingleHost, wrong_command) -{ - std::map service = { - { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "husarion" }, - }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - test_utils.CreateTree("ShutdownSingleHost", service); - auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); -} - -TEST(TestShutdownSingleHost, wrong_user) -{ - std::map service = { - { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "user_what_does_not_exists" }, - }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - test_utils.CreateTree("ShutdownSingleHost", service); - auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From bd066b3f6db35a816a8be746f0250dcaf9a69b32 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Feb 2024 16:30:49 +0000 Subject: [PATCH 16/35] Fixed tesT Signed-off-by: Jakub Delicat --- .../test_shutdown_single_host_plugin.cpp | 38 +++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp index a57a519c5..54c190437 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp @@ -57,17 +57,15 @@ TEST(TestShutdownSingleHost, good_touch_command) std::filesystem::remove(file_path); EXPECT_FALSE(std::filesystem::exists(file_path)); - // #TODO: @delihus change the user to husarion on the panther std::map service = { { "command", "touch " + file_path }, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "deli" }, + { "timeout", "5.0" }, { "user", "husarion" }, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); - testing::internal::CaptureStdout(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); EXPECT_TRUE(std::filesystem::exists(file_path)); @@ -76,6 +74,40 @@ TEST(TestShutdownSingleHost, good_touch_command) test_utils.Stop(); } +TEST(TestShutdownSingleHost, wrong_command) +{ + std::map service = { + { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, + { "timeout", "5.0" }, { "user", "husarion" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + + test_utils.Stop(); +} + +TEST(TestShutdownSingleHost, wrong_user) +{ + std::map service = { + { "command", "echo Hello World!"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, + { "timeout", "5.0" }, { "user", "user_what_does_not_exists" }, + }; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("ShutdownSingleHost", service); + auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + + test_utils.Stop(); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 5b22b3966c674f33eafa7abf8bf0b0a5d04b7ad8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Feb 2024 20:49:25 +0000 Subject: [PATCH 17/35] tested on the robot Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 139 ++++----- panther_manager/README.md | 7 + .../behaviortree_ros2/bt_action_node.hpp | 263 +++++++----------- .../behaviortree_ros2/bt_service_node.hpp | 168 +++++------ .../behaviortree_ros2/bt_topic_pub_node.hpp | 94 +++---- .../behaviortree_ros2/bt_topic_sub_node.hpp | 181 +++++------- .../include/behaviortree_ros2/plugins.hpp | 29 +- .../behaviortree_ros2/ros_node_params.hpp | 6 +- .../action/call_set_bool_service_node.hpp | 11 +- .../call_set_led_animation_service_node.hpp | 19 +- .../action/call_trigger_service_node.hpp | 14 +- .../panther_manager/plugins/shutdown_host.hpp | 160 ++++------- .../plugins/shutdown_hosts_node.hpp | 83 +++--- panther_manager/package.xml | 7 +- panther_manager/src/bt_ros2.cpp | 16 +- .../action/call_set_bool_service_node.cpp | 34 ++- .../call_set_led_animation_service_node.cpp | 42 ++- .../action/call_trigger_service_node.cpp | 35 ++- .../action/shutdown_hosts_from_file_node.cpp | 49 ++-- .../action/shutdown_single_host_node.cpp | 16 +- .../plugins/action/signal_shutdown_node.cpp | 18 +- .../panther_manager_plugin_test_utils.hpp | 41 +-- .../src/panther_manager_plugin_test_utils.cpp | 70 +++-- ...pp => test_call_set_bool_service_node.cpp} | 53 ++-- ...t_call_set_led_animation_service_node.cpp} | 118 ++++---- ...cpp => test_call_trigger_service_node.cpp} | 32 ++- .../test_shutdown_hosts_from_file_node.cpp | 108 +++++++ ...cpp => test_shutdown_single_host_node.cpp} | 48 ++-- ...ugin.cpp => test_signal_shutdown_node.cpp} | 16 +- 29 files changed, 957 insertions(+), 920 deletions(-) create mode 100644 panther_manager/README.md rename panther_manager/test/plugins/{test_set_bool_plugin.cpp => test_call_set_bool_service_node.cpp} (64%) rename panther_manager/test/plugins/{test_set_led_animation_plugin.cpp => test_call_set_led_animation_service_node.cpp} (52%) rename panther_manager/test/plugins/{test_trigger_plugin.cpp => test_call_trigger_service_node.cpp} (75%) create mode 100644 panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp rename panther_manager/test/plugins/{test_shutdown_single_host_plugin.cpp => test_shutdown_single_host_node.cpp} (74%) rename panther_manager/test/plugins/{test_signal_shutdown_plugin.cpp => test_signal_shutdown_node.cpp} (86%) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 1fb889599..e5fd2f824 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -14,7 +14,7 @@ set(PACKAGE_INCLUDE_DEPENDS std_srvs panther_msgs libssh -) + yaml-cpp) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -23,86 +23,89 @@ endforeach() include_directories(include) add_library(behaviortree_ros2 src/bt_ros2.cpp) -add_library(call_set_bool_service_bt_node SHARED src/plugins/action/call_set_bool_service_node.cpp) -list(APPEND plugin_libs call_set_bool_service_bt_node) +add_library(call_set_bool_service_node SHARED + src/plugins/action/call_set_bool_service_node.cpp) +list(APPEND plugin_libs call_set_bool_service_node) -add_library(call_trigger_service_bt_node SHARED src/plugins/action/call_trigger_service_node.cpp) -list(APPEND plugin_libs call_trigger_service_bt_node) +add_library(call_trigger_service_node SHARED + src/plugins/action/call_trigger_service_node.cpp) +list(APPEND plugin_libs call_trigger_service_node) -add_library(call_set_led_animation_service_bt_node SHARED src/plugins/action/call_set_led_animation_service_node.cpp) -list(APPEND plugin_libs call_set_led_animation_service_bt_node) +add_library(call_set_led_animation_service_node SHARED + src/plugins/action/call_set_led_animation_service_node.cpp) +list(APPEND plugin_libs call_set_led_animation_service_node) -add_library(signal_shutdown_bt_node SHARED src/plugins/action/signal_shutdown_node.cpp) -list(APPEND plugin_libs signal_shutdown_bt_node) +add_library(signal_shutdown_node SHARED + src/plugins/action/signal_shutdown_node.cpp) +list(APPEND plugin_libs signal_shutdown_node) -add_library(shutdown_single_host_bt_node SHARED src/plugins/action/shutdown_single_host_node.cpp) -target_link_libraries(shutdown_single_host_bt_node ssh) -list(APPEND plugin_libs shutdown_single_host_bt_node) +add_library(shutdown_single_host_node SHARED + src/plugins/action/shutdown_single_host_node.cpp) +target_link_libraries(shutdown_single_host_node ssh) +list(APPEND plugin_libs shutdown_single_host_node) -foreach(bt_plugin ${plugin_libs}) - target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) - ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS}) -endforeach() +add_library(shutdown_hosts_from_file_node SHARED + src/plugins/action/shutdown_hosts_from_file_node.cpp) +target_link_libraries(shutdown_hosts_from_file_node ssh yaml-cpp) +list(APPEND plugin_libs shutdown_hosts_from_file_node) +foreach(bt_node ${plugin_libs}) + target_compile_definitions(${bt_node} PRIVATE BT_PLUGIN_EXPORT) + ament_target_dependencies(${bt_node} ${PACKAGE_INCLUDE_DEPENDS}) +endforeach() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - add_library(${PROJECT_NAME}_test_plugin_utils test/plugins/src/panther_manager_plugin_test_utils.cpp) - target_include_directories(${PROJECT_NAME}_test_plugin_utils PUBLIC test/plugins/include) - - target_link_libraries(${PROJECT_NAME}_test_plugin_utils - call_set_bool_service_bt_node - call_trigger_service_bt_node - call_set_led_animation_service_bt_node - signal_shutdown_bt_node - shutdown_single_host_bt_node - ) - - ament_target_dependencies( - ${PROJECT_NAME}_test_plugin_utils ${PACKAGE_INCLUDE_DEPENDS} - ) - - ament_add_gtest( - ${PROJECT_NAME}_test_set_bool_plugin - test/plugins/test_set_bool_plugin.cpp - ) - list(APPEND plugin_tests ${PROJECT_NAME}_test_set_bool_plugin) - - ament_add_gtest( - ${PROJECT_NAME}_test_trigger_plugin - test/plugins/test_trigger_plugin.cpp - ) - list(APPEND plugin_tests ${PROJECT_NAME}_test_trigger_plugin) - - ament_add_gtest( - ${PROJECT_NAME}_test_set_led_animation_plugin - test/plugins/test_set_led_animation_plugin.cpp - ) - list(APPEND plugin_tests ${PROJECT_NAME}_test_set_led_animation_plugin) - - ament_add_gtest( - ${PROJECT_NAME}_test_signal_shutdown_plugin - test/plugins/test_signal_shutdown_plugin.cpp - ) - list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_plugin) - - ament_add_gtest( - ${PROJECT_NAME}_test_shutdown_single_host_plugin - test/plugins/test_shutdown_single_host_plugin.cpp - ) - list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_plugin) - - foreach(bt_plugin_test ${plugin_tests}) - target_link_libraries(${bt_plugin_test} ${PROJECT_NAME}_test_plugin_utils) - ament_target_dependencies(${bt_plugin_test} ${PACKAGE_INCLUDE_DEPENDS}) + add_library(${PROJECT_NAME}_test_plugin_utils + test/plugins/src/panther_manager_plugin_test_utils.cpp) + target_include_directories(${PROJECT_NAME}_test_plugin_utils + PUBLIC test/plugins/include) + + target_link_libraries( + ${PROJECT_NAME}_test_plugin_utils + call_set_bool_service_node + call_trigger_service_node + call_set_led_animation_service_node + signal_shutdown_node + shutdown_single_host_node + shutdown_hosts_from_file_node) + + ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils + ${PACKAGE_INCLUDE_DEPENDS}) + + ament_add_gtest(${PROJECT_NAME}_test_call_set_bool_service_node + test/plugins/test_call_set_bool_service_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_call_set_bool_service_node) + + ament_add_gtest(${PROJECT_NAME}_test_call_trigger_service_node + test/plugins/test_call_trigger_service_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_call_trigger_service_node) + + ament_add_gtest(${PROJECT_NAME}_test_call_set_led_animation_service_node + test/plugins/test_call_set_led_animation_service_node.cpp) + list(APPEND plugin_tests + ${PROJECT_NAME}_test_call_set_led_animation_service_node) + + ament_add_gtest(${PROJECT_NAME}_test_signal_shutdown_node + test/plugins/test_signal_shutdown_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_node) + + ament_add_gtest(${PROJECT_NAME}_test_shutdown_single_host_node + test/plugins/test_shutdown_single_host_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_node) + + ament_add_gtest(${PROJECT_NAME}_test_shutdown_hosts_from_file_node + test/plugins/test_shutdown_hosts_from_file_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node) + + foreach(bt_node_test ${plugin_tests}) + target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) + ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS}) endforeach() endif() - -install(TARGETS - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS DESTINATION lib/${PROJECT_NAME}) ament_target_dependencies(behaviortree_ros2 ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/README.md b/panther_manager/README.md new file mode 100644 index 000000000..807fd3ec9 --- /dev/null +++ b/panther_manager/README.md @@ -0,0 +1,7 @@ +# Testing + +For testing you have to copy authenticate ssh key on the localhost: + +```bash +ssh-copy-id -f husarion@localhost +``` diff --git a/panther_manager/include/behaviortree_ros2/bt_action_node.hpp b/panther_manager/include/behaviortree_ros2/bt_action_node.hpp index 8e4d49f8f..274cbae0f 100644 --- a/panther_manager/include/behaviortree_ros2/bt_action_node.hpp +++ b/panther_manager/include/behaviortree_ros2/bt_action_node.hpp @@ -16,9 +16,9 @@ #pragma once #include -#include -#include #include +#include +#include #include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/bt_factory.h" #include "rclcpp_action/rclcpp_action.hpp" @@ -28,8 +28,7 @@ namespace BT { -enum ActionNodeErrorCode -{ +enum ActionNodeErrorCode { SERVER_UNREACHABLE, SEND_GOAL_TIMEOUT, GOAL_REJECTED_BY_SERVER, @@ -38,16 +37,21 @@ enum ActionNodeErrorCode INVALID_GOAL }; -inline const char* toStr(const ActionNodeErrorCode& err) +inline const char * toStr(const ActionNodeErrorCode & err) { - switch (err) - { - case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; - case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; - case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; - case ACTION_ABORTED: return "ACTION_ABORTED"; - case ACTION_CANCELLED: return "ACTION_CANCELLED"; - case INVALID_GOAL: return "INVALID_GOAL"; + switch (err) { + case SERVER_UNREACHABLE: + return "SERVER_UNREACHABLE"; + case SEND_GOAL_TIMEOUT: + return "SEND_GOAL_TIMEOUT"; + case GOAL_REJECTED_BY_SERVER: + return "GOAL_REJECTED_BY_SERVER"; + case ACTION_ABORTED: + return "ACTION_ABORTED"; + case ACTION_CANCELLED: + return "ACTION_CANCELLED"; + case INVALID_GOAL: + return "INVALID_GOAL"; } return nullptr; } @@ -70,10 +74,9 @@ inline const char* toStr(const ActionNodeErrorCode& err) * 1. If a value is passes in the InputPort "action_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosActionNode : public BT::ActionNodeBase { - public: // Type definitions using ActionType = ActionT; @@ -89,9 +92,8 @@ class RosActionNode : public BT::ActionNodeBase * factory.registerNodeType<>(node_name, params); * */ - explicit RosActionNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); + explicit RosActionNode( + const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); virtual ~RosActionNode() = default; @@ -105,8 +107,7 @@ class RosActionNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("action_name", "__default__placeholder__", "Action server name") - }; + InputPort("action_name", "__default__placeholder__", "Action server name")}; basic.insert(addition.begin(), addition.end()); return basic; } @@ -115,10 +116,7 @@ class RosActionNode : public BT::ActionNodeBase * @brief Creates list of BT ports * @return PortsList Containing basic ports along with node-specific ports */ - static PortsList providedPorts() - { - return providedBasicPorts({}); - } + static PortsList providedPorts() { return providedBasicPorts({}); } /// @brief Callback executed when the node is halted. Note that cancelGoal() /// is done automatically. @@ -132,12 +130,12 @@ class RosActionNode : public BT::ActionNodeBase * @return false if the request should not be sent. In that case, * RosActionNode::onFailure(INVALID_GOAL) will be called. */ - virtual bool setGoal(Goal& goal) = 0; + virtual bool setGoal(Goal & goal) = 0; /** Callback invoked when the result is received by the server. * It is up to the user to define if the action returns SUCCESS or FAILURE. */ - virtual BT::NodeStatus onResultReceived(const WrappedResult& result) = 0; + virtual BT::NodeStatus onResultReceived(const WrappedResult & result) = 0; /** Callback invoked when the feedback is received. * It generally returns RUNNING, but the user can also use this callback to cancel the @@ -151,22 +149,17 @@ class RosActionNode : public BT::ActionNodeBase /** Callback invoked when something goes wrong. * It must return either SUCCESS or FAILURE. */ - virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) - { - return NodeStatus::FAILURE; - } + virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) { return NodeStatus::FAILURE; } /// Method used to send a request to the Action server to cancel the current goal void cancelGoal(); - /// The default halt() implementation will call cancelGoal if necessary. void halt() override final; NodeStatus tick() override final; protected: - std::shared_ptr node_; std::string prev_action_name_; bool action_name_may_change_ = false; @@ -174,7 +167,6 @@ class RosActionNode : public BT::ActionNodeBase const std::chrono::milliseconds wait_for_server_timeout_; private: - ActionClientPtr action_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; @@ -187,18 +179,17 @@ class RosActionNode : public BT::ActionNodeBase bool goal_received_; WrappedResult result_; - bool createClient(const std::string &action_name); + bool createClient(const std::string & action_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosActionNode::RosActionNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams ¶ms): - BT::ActionNodeBase(instance_name, conf), +template +inline RosActionNode::RosActionNode( + const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) +: BT::ActionNodeBase(instance_name, conf), node_(params.nh), server_timeout_(params.server_timeout), wait_for_server_timeout_(params.wait_for_server_timeout) @@ -210,49 +201,39 @@ template inline // check port remapping auto portIt = config().input_ports.find("action_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_action_name = portIt->second; + if (portIt != config().input_ports.end()) { + const std::string & bb_action_name = portIt->second; - if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { + if (bb_action_name.empty() || bb_action_name == "__default__placeholder__") { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createClient(params.default_port_value); } - } - else if(!isBlackboardPointer(bb_action_name)) - { + } else if (!isBlackboardPointer(bb_action_name)) { // If the content of the port "action_name" is not // a pointer to the blackboard, but a static string, we can // create the client in the constructor. createClient(bb_action_name); - } - else { + } else { action_name_may_change_ = true; // createClient will be invoked in the first tick(). } - } - else { - - if(params.default_port_value.empty()) { + } else { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createClient(params.default_port_value); } } } -template inline - bool RosActionNode::createClient(const std::string& action_name) +template +inline bool RosActionNode::createClient(const std::string & action_name) { - if(action_name.empty()) - { + if (action_name.empty()) { throw RuntimeError("action_name is empty"); } @@ -263,43 +244,38 @@ template inline prev_action_name_ = action_name; bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); - if(!found) - { - RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", - name().c_str(), prev_action_name_.c_str()); + if (!found) { + RCLCPP_ERROR( + node_->get_logger(), "%s: Action server with name '%s' is not reachable.", name().c_str(), + prev_action_name_.c_str()); } return found; } -template inline - NodeStatus RosActionNode::tick() +template +inline NodeStatus RosActionNode::tick() { // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client - if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) - { + if (!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) { std::string action_name; getInput("action_name", action_name); - if(prev_action_name_ != action_name) - { + if (prev_action_name_ != action_name) { createClient(action_name); } } //------------------------------------------ - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { + auto CheckStatus = [](NodeStatus status) { + if (!isStatusCompleted(status)) { throw std::logic_error("RosActionNode: the callback must return either SUCCESS of FAILURE"); } return status; }; // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { + if (status() == BT::NodeStatus::IDLE) { setStatus(NodeStatus::RUNNING); goal_received_ = false; @@ -309,146 +285,121 @@ template inline Goal goal; - if( !setGoal(goal) ) - { - return CheckStatus( onFailure(INVALID_GOAL) ); + if (!setGoal(goal)) { + return CheckStatus(onFailure(INVALID_GOAL)); } typename ActionClient::SendGoalOptions goal_options; //-------------------- goal_options.feedback_callback = - [this](typename GoalHandle::SharedPtr, - const std::shared_ptr feedback) - { - on_feedback_state_change_ = onFeedback(feedback); - if( on_feedback_state_change_ == NodeStatus::IDLE) - { - throw std::logic_error("onFeedback must not return IDLE"); - } - emitWakeUpSignal(); - }; + [this](typename GoalHandle::SharedPtr, const std::shared_ptr feedback) { + on_feedback_state_change_ = onFeedback(feedback); + if (on_feedback_state_change_ == NodeStatus::IDLE) { + throw std::logic_error("onFeedback must not return IDLE"); + } + emitWakeUpSignal(); + }; //-------------------- - goal_options.result_callback = - [this](const WrappedResult& result) - { + goal_options.result_callback = [this](const WrappedResult & result) { if (goal_handle_->get_goal_id() == result.goal_id) { - RCLCPP_DEBUG( node_->get_logger(), "result_callback" ); + RCLCPP_DEBUG(node_->get_logger(), "result_callback"); result_ = result; emitWakeUpSignal(); } }; //-------------------- goal_options.goal_response_callback = - [this](typename GoalHandle::SharedPtr const future_handle) - { - auto goal_handle_ = future_handle.get(); - if (!goal_handle_) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); - } - }; + [this](typename GoalHandle::SharedPtr const future_handle) { + auto goal_handle_ = future_handle.get(); + if (!goal_handle_) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); + } + }; //-------------------- - future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); + future_goal_handle_ = action_client_->async_send_goal(goal, goal_options); time_goal_sent_ = node_->now(); return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) - { + if (status() == NodeStatus::RUNNING) { callback_group_executor_.spin_some(); // FIRST case: check if the goal request has a timeout - if( !goal_received_ ) - { + if (!goal_received_) { auto nodelay = std::chrono::milliseconds(0); - auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); + auto timeout = rclcpp::Duration::from_seconds(double(server_timeout_.count()) / 1000); auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); - if (ret != rclcpp::FutureReturnCode::SUCCESS) - { - if( (node_->now() - time_goal_sent_) > timeout ) - { - return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); - } - else{ + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + if ((node_->now() - time_goal_sent_) > timeout) { + return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); + } else { return NodeStatus::RUNNING; } - } - else - { + } else { goal_received_ = true; goal_handle_ = future_goal_handle_.get(); future_goal_handle_ = {}; if (!goal_handle_) { - return CheckStatus( onFailure( GOAL_REJECTED_BY_SERVER ) ); + return CheckStatus(onFailure(GOAL_REJECTED_BY_SERVER)); } } } // SECOND case: onFeedback requested a stop - if( on_feedback_state_change_ != NodeStatus::RUNNING ) - { + if (on_feedback_state_change_ != NodeStatus::RUNNING) { cancelGoal(); return on_feedback_state_change_; } // THIRD case: result received, requested a stop - if( result_.code != rclcpp_action::ResultCode::UNKNOWN) - { - if( result_.code == rclcpp_action::ResultCode::ABORTED ) - { - return CheckStatus( onFailure( ACTION_ABORTED ) ); - } - else if( result_.code == rclcpp_action::ResultCode::CANCELED ) - { - return CheckStatus( onFailure( ACTION_CANCELLED ) ); - } - else{ - return CheckStatus( onResultReceived( result_ ) ); + if (result_.code != rclcpp_action::ResultCode::UNKNOWN) { + if (result_.code == rclcpp_action::ResultCode::ABORTED) { + return CheckStatus(onFailure(ACTION_ABORTED)); + } else if (result_.code == rclcpp_action::ResultCode::CANCELED) { + return CheckStatus(onFailure(ACTION_CANCELLED)); + } else { + return CheckStatus(onResultReceived(result_)); } } } return NodeStatus::RUNNING; } -template inline - void RosActionNode::halt() +template +inline void RosActionNode::halt() { - if(status() == BT::NodeStatus::RUNNING) - { + if (status() == BT::NodeStatus::RUNNING) { cancelGoal(); onHalt(); } } -template inline - void RosActionNode::cancelGoal() +template +inline void RosActionNode::cancelGoal() { auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR( node_->get_logger(), "Failed to cancel action server for [%s]", - prev_action_name_.c_str()); + if ( + callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to cancel action server for [%s]", prev_action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", - prev_action_name_.c_str()); + if ( + callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to get result call failed :( for [%s]", + prev_action_name_.c_str()); } } - - - } // namespace BT - diff --git a/panther_manager/include/behaviortree_ros2/bt_service_node.hpp b/panther_manager/include/behaviortree_ros2/bt_service_node.hpp index 66477e3b2..bf0ea41b6 100644 --- a/panther_manager/include/behaviortree_ros2/bt_service_node.hpp +++ b/panther_manager/include/behaviortree_ros2/bt_service_node.hpp @@ -16,9 +16,9 @@ #pragma once #include -#include -#include #include +#include +#include #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/ros_node_params.hpp" @@ -26,22 +26,24 @@ namespace BT { -enum ServiceNodeErrorCode -{ +enum ServiceNodeErrorCode { SERVICE_UNREACHABLE, SERVICE_TIMEOUT, INVALID_REQUEST, SERVICE_ABORTED }; -inline const char* toStr(const ServiceNodeErrorCode& err) +inline const char * toStr(const ServiceNodeErrorCode & err) { - switch (err) - { - case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; - case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; - case INVALID_REQUEST: return "INVALID_REQUEST"; - case SERVICE_ABORTED: return "SERVICE_ABORTED"; + switch (err) { + case SERVICE_UNREACHABLE: + return "SERVICE_UNREACHABLE"; + case SERVICE_TIMEOUT: + return "SERVICE_TIMEOUT"; + case INVALID_REQUEST: + return "INVALID_REQUEST"; + case SERVICE_ABORTED: + return "SERVICE_ABORTED"; } return nullptr; } @@ -64,10 +66,9 @@ inline const char* toStr(const ServiceNodeErrorCode& err) * 1. If a value is passes in the InputPort "service_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosServiceNode : public BT::ActionNodeBase { - public: // Type definitions using ServiceClient = typename rclcpp::Client; @@ -78,9 +79,9 @@ class RosServiceNode : public BT::ActionNodeBase * * factory.registerNodeType<>(node_name, params); */ - explicit RosServiceNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const BT::RosNodeParams& params); + explicit RosServiceNode( + const std::string & instance_name, const BT::NodeConfig & conf, + const BT::RosNodeParams & params); virtual ~RosServiceNode() = default; @@ -94,8 +95,7 @@ class RosServiceNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("service_name", "__default__placeholder__", "Service name") - }; + InputPort("service_name", "__default__placeholder__", "Service name")}; basic.insert(addition.begin(), addition.end()); return basic; } @@ -104,10 +104,7 @@ class RosServiceNode : public BT::ActionNodeBase * @brief Creates list of BT ports * @return PortsList Containing basic ports along with node-specific ports */ - static PortsList providedPorts() - { - return providedBasicPorts({}); - } + static PortsList providedPorts() { return providedBasicPorts({}); } NodeStatus tick() override final; @@ -122,23 +119,19 @@ class RosServiceNode : public BT::ActionNodeBase * @return false if the request should not be sent. In that case, * RosServiceNode::onFailure(INVALID_REQUEST) will be called. */ - virtual bool setRequest(typename Request::SharedPtr& request) = 0; + virtual bool setRequest(typename Request::SharedPtr & request) = 0; /** Callback invoked when the response is received by the server. * It is up to the user to define if this returns SUCCESS or FAILURE. */ - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) = 0; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) = 0; /** Callback invoked when something goes wrong; you can override it. * It must return either SUCCESS or FAILURE. */ - virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { - return NodeStatus::FAILURE; - } + virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) { return NodeStatus::FAILURE; } protected: - std::shared_ptr node_; std::string prev_service_name_; bool service_name_may_change_ = false; @@ -146,7 +139,6 @@ class RosServiceNode : public BT::ActionNodeBase const std::chrono::milliseconds wait_for_service_timeout_; private: - typename std::shared_ptr service_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; @@ -158,112 +150,97 @@ class RosServiceNode : public BT::ActionNodeBase bool response_received_; typename Response::SharedPtr response_; - bool createClient(const std::string &service_name); + bool createClient(const std::string & service_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosServiceNode::RosServiceNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params): - BT::ActionNodeBase(instance_name, conf), +template +inline RosServiceNode::RosServiceNode( + const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) +: BT::ActionNodeBase(instance_name, conf), node_(params.nh), service_timeout_(params.server_timeout), wait_for_service_timeout_(params.wait_for_server_timeout) { // check port remapping auto portIt = config().input_ports.find("service_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_service_name = portIt->second; + if (portIt != config().input_ports.end()) { + const std::string & bb_service_name = portIt->second; - if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { + if (bb_service_name.empty() || bb_service_name == "__default__placeholder__") { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createClient(params.default_port_value); } - } - else if(!isBlackboardPointer(bb_service_name)) - { + } else if (!isBlackboardPointer(bb_service_name)) { // If the content of the port "service_name" is not // a pointer to the blackboard, but a static string, we can // create the client in the constructor. createClient(bb_service_name); - } - else { + } else { service_name_may_change_ = true; // createClient will be invoked in the first tick(). } - } - else { - - if(params.default_port_value.empty()) { + } else { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createClient(params.default_port_value); } } } -template inline - bool RosServiceNode::createClient(const std::string& service_name) +template +inline bool RosServiceNode::createClient(const std::string & service_name) { - if(service_name.empty()) - { + if (service_name.empty()) { throw RuntimeError("service_name is empty"); } callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); + service_client_ = node_->create_client( + service_name, rmw_qos_profile_services_default, callback_group_); prev_service_name_ = service_name; bool found = service_client_->wait_for_service(wait_for_service_timeout_); - if(!found) - { - RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", - name().c_str(), prev_service_name_.c_str()); + if (!found) { + RCLCPP_ERROR( + node_->get_logger(), "%s: Service with name '%s' is not reachable.", name().c_str(), + prev_service_name_.c_str()); } return found; } -template inline - NodeStatus RosServiceNode::tick() +template +inline NodeStatus RosServiceNode::tick() { // First, check if the service_client_ is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client - if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) - { + if (!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) { std::string service_name; getInput("service_name", service_name); - if(prev_service_name_ != service_name) - { + if (prev_service_name_ != service_name) { createClient(service_name); } } - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { + auto CheckStatus = [](NodeStatus status) { + if (!isStatusCompleted(status)) { throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or FAILURE"); } return status; }; // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { + if (status() == BT::NodeStatus::IDLE) { setStatus(NodeStatus::RUNNING); response_received_ = false; @@ -273,9 +250,8 @@ template inline typename Request::SharedPtr request = std::make_shared(); - if( !setRequest(request) ) - { - return CheckStatus( onFailure(INVALID_REQUEST) ); + if (!setRequest(request)) { + return CheckStatus(onFailure(INVALID_REQUEST)); } future_response_ = service_client_->async_send_request(request).share(); @@ -284,30 +260,23 @@ template inline return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) - { + if (status() == NodeStatus::RUNNING) { callback_group_executor_.spin_some(); // FIRST case: check if the goal request has a timeout - if( !response_received_ ) - { + if (!response_received_) { auto const nodelay = std::chrono::milliseconds(0); - auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); + auto const timeout = rclcpp::Duration::from_seconds(double(service_timeout_.count()) / 1000); auto ret = callback_group_executor_.spin_until_future_complete(future_response_, nodelay); - if (ret != rclcpp::FutureReturnCode::SUCCESS) - { - if( (node_->now() - time_request_sent_) > timeout ) - { - return CheckStatus( onFailure(SERVICE_TIMEOUT) ); - } - else{ + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + if ((node_->now() - time_request_sent_) > timeout) { + return CheckStatus(onFailure(SERVICE_TIMEOUT)); + } else { return NodeStatus::RUNNING; } - } - else - { + } else { response_received_ = true; response_ = future_response_.get(); future_response_ = {}; @@ -319,20 +288,17 @@ template inline } // SECOND case: response received - return CheckStatus( onResponseReceived( response_ ) ); + return CheckStatus(onResponseReceived(response_)); } return NodeStatus::RUNNING; } -template inline - void RosServiceNode::halt() +template +inline void RosServiceNode::halt() { - if( status() == NodeStatus::RUNNING ) - { + if (status() == NodeStatus::RUNNING) { resetStatus(); } } - } // namespace BT - diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp index ebeb1f5b0..41d01060a 100644 --- a/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -15,11 +15,11 @@ #pragma once #include -#include -#include #include -#include "behaviortree_cpp/condition_node.h" +#include +#include #include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/condition_node.h" #include "behaviortree_ros2/ros_node_params.hpp" namespace BT @@ -29,10 +29,9 @@ namespace BT * @brief Abstract class to wrap a ROS publisher * */ -template +template class RosTopicPubNode : public BT::ConditionNode { - public: // Type definitions using Publisher = typename rclcpp::Publisher; @@ -44,9 +43,8 @@ class RosTopicPubNode : public BT::ConditionNode * * Note that if the external_action_client is not set, the constructor will build its own. * */ - explicit RosTopicPubNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); + explicit RosTopicPubNode( + const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); virtual ~RosTopicPubNode() = default; @@ -60,8 +58,7 @@ class RosTopicPubNode : public BT::ConditionNode static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name") - }; + InputPort("topic_name", "__default__placeholder__", "Topic name")}; basic.insert(addition.begin(), addition.end()); return basic; } @@ -70,10 +67,7 @@ class RosTopicPubNode : public BT::ConditionNode * @brief Creates list of BT ports * @return PortsList Containing basic ports along with node-specific ports */ - static PortsList providedPorts() - { - return providedBasicPorts({}); - } + static PortsList providedPorts() { return providedBasicPorts({}); } NodeStatus tick() override final; @@ -85,103 +79,86 @@ class RosTopicPubNode : public BT::ConditionNode * @return return false if anything is wrong and we must not send the message. * the Condition will return FAILURE. */ - virtual bool setMessage(TopicT& msg) = 0; + virtual bool setMessage(TopicT & msg) = 0; protected: - std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; private: - std::shared_ptr publisher_; - bool createPublisher(const std::string& topic_name); + bool createPublisher(const std::string & topic_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosTopicPubNode::RosTopicPubNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ +template +inline RosTopicPubNode::RosTopicPubNode( + const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) +: BT::ConditionNode(instance_name, conf), node_(params.nh) +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_topic_name = portIt->second; + if (portIt != config().input_ports.end()) { + const std::string & bb_topic_name = portIt->second; - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { + if (bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createPublisher(params.default_port_value); } - } - else if(!isBlackboardPointer(bb_topic_name)) - { + } else if (!isBlackboardPointer(bb_topic_name)) { // If the content of the port "topic_name" is not // a pointer to the blackboard, but a static string, we can // create the client in the constructor. createPublisher(bb_topic_name); - } - else { + } else { topic_name_may_change_ = true; // createPublisher will be invoked in the first tick(). } - } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { + if (params.default_port_value.empty()) { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } else { createPublisher(params.default_port_value); } } } -template inline - bool RosTopicPubNode::createPublisher(const std::string& topic_name) +template +inline bool RosTopicPubNode::createPublisher(const std::string & topic_name) { - if(topic_name.empty()) - { + if (topic_name.empty()) { throw RuntimeError("topic_name is empty"); } - + publisher_ = node_->create_publisher(topic_name, 1); prev_topic_name_ = topic_name; return true; } -template inline - NodeStatus RosTopicPubNode::tick() +template +inline NodeStatus RosTopicPubNode::tick() { // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber - if(!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) - { + if (!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) { std::string topic_name; getInput("topic_name", topic_name); - if(prev_topic_name_ != topic_name) - { + if (prev_topic_name_ != topic_name) { createPublisher(topic_name); } } T msg; - if (!setMessage(msg)) - { + if (!setMessage(msg)) { return NodeStatus::FAILURE; } publisher_->publish(msg); @@ -189,4 +166,3 @@ template inline } } // namespace BT - diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp index 311594b67..6d3570586 100644 --- a/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -15,19 +15,18 @@ #pragma once #include -#include -#include #include -#include "behaviortree_cpp/condition_node.h" +#include +#include #include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_ros2/ros_node_params.hpp" #include +#include "behaviortree_ros2/ros_node_params.hpp" namespace BT { - /** * @brief Abstract class to wrap a Topic subscriber. * Considering the example in the tutorial: @@ -42,53 +41,48 @@ namespace BT * 1. If a value is passes in the InputPort "topic_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosTopicSubNode : public BT::ConditionNode { - public: +public: // Type definitions using Subscriber = typename rclcpp::Subscription; - protected: +protected: struct SubscriberInstance { - void init(std::shared_ptr node, const std::string& topic_name) + void init(std::shared_ptr node, const std::string & topic_name) { // create a callback group for this particular instance - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group( - callback_group, node->get_node_base_interface()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group(callback_group, node->get_node_base_interface()); rclcpp::SubscriptionOptions option; option.callback_group = callback_group; - // The callback will broadcast to all the instances of RosTopicSubNode - auto callback = [this](const std::shared_ptr msg) - { - broadcaster(msg); - }; - subscriber = node->create_subscription(topic_name, 1, callback, option); + // The callback will broadcast to all the instances of RosTopicSubNode + auto callback = [this](const std::shared_ptr msg) { broadcaster(msg); }; + subscriber = node->create_subscription(topic_name, 1, callback, option); } std::shared_ptr subscriber; rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; - boost::signals2::signal)> broadcaster; - - + boost::signals2::signal)> broadcaster; }; - static std::mutex& registryMutex() + static std::mutex & registryMutex() { static std::mutex sub_mutex; return sub_mutex; } // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map>& getRegistry() + static std::unordered_map> & getRegistry() { - static std::unordered_map> subscribers_registry; + static std::unordered_map> + subscribers_registry; return subscribers_registry; } @@ -99,13 +93,9 @@ class RosTopicSubNode : public BT::ConditionNode boost::signals2::connection signal_connection_; std::string subscriber_key_; - rclcpp::Logger logger() - { - return node_->get_logger(); - } - - public: + rclcpp::Logger logger() { return node_->get_logger(); } +public: /** You are not supposed to instantiate this class directly, the factory will do it. * To register this class into the factory, use: * @@ -113,27 +103,24 @@ class RosTopicSubNode : public BT::ConditionNode * * Note that if the external_action_client is not set, the constructor will build its own. * */ - explicit RosTopicSubNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); + explicit RosTopicSubNode( + const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); - virtual ~RosTopicSubNode() + virtual ~RosTopicSubNode() { signal_connection_.disconnect(); // remove the subscribers from the static registry when the ALL the // instances of RosTopicSubNode are destroyed (i.e., the tree is destroyed) - if(sub_instance_) - { + if (sub_instance_) { sub_instance_.reset(); std::unique_lock lk(registryMutex()); - auto& registry = getRegistry(); + auto & registry = getRegistry(); auto it = registry.find(subscriber_key_); // when the reference count is 1, means that the only instance is owned by the // registry itself. Delete it - if(it != registry.end() && it->second.use_count() <= 1) - { + if (it != registry.end() && it->second.use_count() <= 1) { registry.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); + RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str()); } } } @@ -147,8 +134,7 @@ class RosTopicSubNode : public BT::ConditionNode static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name") - }; + InputPort("topic_name", "__default__placeholder__", "Topic name")}; basic.insert(addition.begin(), addition.end()); return basic; } @@ -157,10 +143,7 @@ class RosTopicSubNode : public BT::ConditionNode * @brief Creates list of BT ports * @return PortsList Containing basic ports along with node-specific ports */ - static PortsList providedPorts() - { - return providedBasicPorts({}); - } + static PortsList providedPorts() { return providedBasicPorts({}); } NodeStatus tick() override final; @@ -168,75 +151,61 @@ class RosTopicSubNode : public BT::ConditionNode * * @param last_msg the latest message received, since the last tick. * Will be empty if no new message received. - * + * * @return the new status of the Node, based on last_msg */ - virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + virtual NodeStatus onTick(const std::shared_ptr & last_msg) = 0; private: - - bool createSubscriber(const std::string& topic_name); + bool createSubscriber(const std::string & topic_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ +template +inline RosTopicSubNode::RosTopicSubNode( + const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) +: BT::ConditionNode(instance_name, conf), node_(params.nh) +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_topic_name = portIt->second; + if (portIt != config().input_ports.end()) { + const std::string & bb_topic_name = portIt->second; - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { + if (bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { + if (params.default_port_value.empty()) { throw std::logic_error( "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { createSubscriber(params.default_port_value); } - } - else if(!isBlackboardPointer(bb_topic_name)) - { + } else if (!isBlackboardPointer(bb_topic_name)) { // If the content of the port "topic_name" is not // a pointer to the blackboard, but a static string, we can // create the client in the constructor. createSubscriber(bb_topic_name); - } - else { + } else { // do nothing // createSubscriber will be invoked in the first tick(). } - } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { + } else { + if (params.default_port_value.empty()) { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } else { createSubscriber(params.default_port_value); } } } -template inline - bool RosTopicSubNode::createSubscriber(const std::string& topic_name) +template +inline bool RosTopicSubNode::createSubscriber(const std::string & topic_name) { - if(topic_name.empty()) - { + if (topic_name.empty()) { throw RuntimeError("topic_name is empty"); } - if(sub_instance_) - { + if (sub_instance_) { throw RuntimeError("Can't call createSubscriber more than once"); } @@ -244,60 +213,52 @@ template inline std::unique_lock lk(registryMutex()); subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; - auto& registry = getRegistry(); + auto & registry = getRegistry(); auto it = registry.find(subscriber_key_); - if(it == registry.end()) - { - it = registry.insert( {subscriber_key_, std::make_shared() }).first; + if (it == registry.end()) { + it = registry.insert({subscriber_key_, std::make_shared()}).first; sub_instance_ = it->second; sub_instance_->init(node_, topic_name); - RCLCPP_INFO(logger(), - "Node [%s] created Subscriber to topic [%s]", - name().c_str(), topic_name.c_str() ); - } - else { + RCLCPP_INFO( + logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), topic_name.c_str()); + } else { sub_instance_ = it->second; } - // add "this" as received of the broadcaster - signal_connection_ = sub_instance_->broadcaster.connect( - [this](const std::shared_ptr msg) { last_msg_ = msg; } ); + signal_connection_ = + sub_instance_->broadcaster.connect([this](const std::shared_ptr msg) { last_msg_ = msg; }); topic_name_ = topic_name; return true; } - -template inline - NodeStatus RosTopicSubNode::tick() +template +inline NodeStatus RosTopicSubNode::tick() { // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber - if(!sub_instance_) - { + if (!sub_instance_) { std::string topic_name; getInput("topic_name", topic_name); - createSubscriber(topic_name); + createSubscriber(topic_name); } - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { - throw std::logic_error("RosTopicSubNode: the callback must return" - "either SUCCESS or FAILURE"); + auto CheckStatus = [](NodeStatus status) { + if (!isStatusCompleted(status)) { + throw std::logic_error( + "RosTopicSubNode: the callback must return" + "either SUCCESS or FAILURE"); } return status; }; sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus (onTick(last_msg_)); + auto status = CheckStatus(onTick(last_msg_)); last_msg_ = nullptr; return status; } } // namespace BT - diff --git a/panther_manager/include/behaviortree_ros2/plugins.hpp b/panther_manager/include/behaviortree_ros2/plugins.hpp index d94f52933..a7b19510f 100644 --- a/panther_manager/include/behaviortree_ros2/plugins.hpp +++ b/panther_manager/include/behaviortree_ros2/plugins.hpp @@ -20,7 +20,6 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/ros_node_params.hpp" - // Use this macro to generate a plugin for: // // - BT::RosActionNode @@ -34,14 +33,12 @@ // Usage example: // CreateRosNodePlugin(MyClassName, "MyClassName"); -#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ -BTCPP_EXPORT void \ -BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ - const BT::RosNodeParams& params) \ -{ \ - factory.registerNodeType(REGISTRATION_NAME, params); \ -} \ - +#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ + BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin( \ + BT::BehaviorTreeFactory & factory, const BT::RosNodeParams & params) \ + { \ + factory.registerNodeType(REGISTRATION_NAME, params); \ + } /** * @brief RegisterRosNode function used to load a plugin and register @@ -51,18 +48,12 @@ BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ * @param filepath path to the plugin. * @param params parameters to pass to the instances of the Node. */ -inline -void RegisterRosNode(BT::BehaviorTreeFactory& factory, - const std::filesystem::path& filepath, - const BT::RosNodeParams& params) +inline void RegisterRosNode( + BT::BehaviorTreeFactory & factory, const std::filesystem::path & filepath, + const BT::RosNodeParams & params) { BT::SharedLibrary loader(filepath.generic_string()); - typedef void (*Func)(BT::BehaviorTreeFactory&, - const BT::RosNodeParams&); + typedef void (*Func)(BT::BehaviorTreeFactory &, const BT::RosNodeParams &); auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); func(factory, params); } - - - - diff --git a/panther_manager/include/behaviortree_ros2/ros_node_params.hpp b/panther_manager/include/behaviortree_ros2/ros_node_params.hpp index 148c086d3..0802350b8 100644 --- a/panther_manager/include/behaviortree_ros2/ros_node_params.hpp +++ b/panther_manager/include/behaviortree_ros2/ros_node_params.hpp @@ -14,10 +14,10 @@ #pragma once -#include -#include #include #include +#include +#include namespace BT { @@ -42,4 +42,4 @@ struct RosNodeParams std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); }; -} +} // namespace BT diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index cbd8215e0..3b0923a48 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -25,18 +25,19 @@ namespace panther_manager class CallSetBoolService : public BT::RosServiceNode { public: - CallSetBoolService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) - : BT::RosServiceNode(name, conf, params) + CallSetBoolService( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) { } static BT::PortsList providedPorts() { - return providedBasicPorts({ BT::InputPort("data", "true / false value") }); + return providedBasicPorts({BT::InputPort("data", "true / false value")}); } - virtual bool setRequest(typename Request::SharedPtr& request) override; - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; + virtual bool setRequest(typename Request::SharedPtr & request) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 9cd59cd9e..4ef263e23 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -16,8 +16,8 @@ #define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ #include -#include #include +#include namespace panther_manager { @@ -25,24 +25,25 @@ namespace panther_manager class CallSetLedAnimationService : public BT::RosServiceNode { public: - CallSetLedAnimationService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) - : BT::RosServiceNode(name, conf, params) + CallSetLedAnimationService( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) { } static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("id", "animation ID"), - BT::InputPort("param", "optional parameter"), - BT::InputPort("repeating", "indicates if animation should repeat"), + BT::InputPort("id", "animation ID"), + BT::InputPort("param", "optional parameter"), + BT::InputPort("repeating", "indicates if animation should repeat"), }); } - virtual bool setRequest(typename Request::SharedPtr& request) override; - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; + virtual bool setRequest(typename Request::SharedPtr & request) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; } // namespace panther_manager -#endif // PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#endif // PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp index 120df2008..4bc6607b0 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp @@ -25,18 +25,16 @@ namespace panther_manager class CallTriggerService : public BT::RosServiceNode { public: - CallTriggerService(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) - : BT::RosServiceNode(name, conf, params) + CallTriggerService( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) { } - static BT::PortsList providedPorts() - { - return providedBasicPorts({}); - } + static BT::PortsList providedPorts() { return providedBasicPorts({}); } - virtual bool setRequest(typename Request::SharedPtr& /* request*/ ) override; - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; + virtual bool setRequest(typename Request::SharedPtr & /* request*/) override; + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index 382a26fba..91caa7a36 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -15,21 +15,20 @@ #ifndef PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ #define PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ +#include #include #include #include #include #include #include -#include #include namespace panther_manager { -enum class ShutdownHostState -{ +enum class ShutdownHostState { IDLE = 0, COMMAND_EXECUTED, RESPONSE_RECEIVED, @@ -44,50 +43,44 @@ class ShutdownHost public: // default constructor ShutdownHost() - : ip_("") - , user_("") - , port_(22) - , command_("") - , timeout_(5000) - , ping_for_success_(true) - , hash_(std::hash{}("")) + : ip_(""), + user_(""), + port_(22), + command_(""), + timeout_(5000), + ping_for_success_(true), + hash_(std::hash{}("")) { } - ShutdownHost(const std::string ip, const std::string user, const int port = 22, - const std::string command = "sudo shutdown now", const float timeout = 5.0, - const bool ping_for_success = true) - : ip_(ip) - , user_(user) - , port_(port) - , command_(command) - , timeout_(static_cast(timeout * 1000)) - , ping_for_success_(ping_for_success) - , hash_(std::hash{}(ip + user + std::to_string(port))) - , state_(ShutdownHostState::IDLE) + ShutdownHost( + const std::string ip, const std::string user, const int port = 22, + const std::string command = "sudo shutdown now", const float timeout = 5.0, + const bool ping_for_success = true) + : ip_(ip), + user_(user), + port_(port), + command_(command), + timeout_(static_cast(timeout * 1000)), + ping_for_success_(ping_for_success), + hash_(std::hash{}(ip + user + std::to_string(port))), + state_(ShutdownHostState::IDLE) { } - ~ShutdownHost() - { - } + ~ShutdownHost() {} void call() { - switch (state_) - { + switch (state_) { case ShutdownHostState::IDLE: - if (!is_available()) - { + if (!is_available()) { state_ = ShutdownHostState::SKIPPED; break; } - try - { + try { request_shutdown(); - } - catch (const std::runtime_error &err) - { + } catch (const std::runtime_error & err) { state_ = ShutdownHostState::FAILURE; failure_reason_ = err.what(); break; @@ -96,15 +89,11 @@ class ShutdownHost break; case ShutdownHostState::COMMAND_EXECUTED: - try - { - if (update_response()) - { + try { + if (update_response()) { break; } - } - catch (const std::runtime_error &err) - { + } catch (const std::runtime_error & err) { state_ = ShutdownHostState::FAILURE; failure_reason_ = err.what(); break; @@ -116,13 +105,11 @@ class ShutdownHost state_ = ShutdownHostState::PINGING; break; case ShutdownHostState::PINGING: - if (ping_for_success_ ? !is_available() : true) - { + if (ping_for_success_ ? !is_available() : true) { state_ = ShutdownHostState::SUCCESS; break; } - if (timeout_exceeded()) - { + if (timeout_exceeded()) { state_ = ShutdownHostState::FAILURE; failure_reason_ = "Timeout exceeded"; } @@ -140,8 +127,7 @@ class ShutdownHost void close_connection() { - if (ssh_channel_is_closed(channel_)) - { + if (ssh_channel_is_closed(channel_)) { return; } @@ -152,55 +138,25 @@ class ShutdownHost ssh_free(session_); } - int get_port() const - { - return port_; - } + int get_port() const { return port_; } - std::string get_ip() const - { - return ip_; - } + std::string get_ip() const { return ip_; } - std::string get_user() const - { - return user_; - } + std::string get_user() const { return user_; } - std::string get_command() const - { - return command_; - } + std::string get_command() const { return command_; } - std::string get_error() const - { - return failure_reason_; - } + std::string get_error() const { return failure_reason_; } - std::string get_response() const - { - return output_; - } + std::string get_response() const { return output_; } - ShutdownHostState get_state() const - { - return state_; - } + ShutdownHostState get_state() const { return state_; } - bool operator==(const ShutdownHost& other) const - { - return hash_ == other.hash_; - } + bool operator==(const ShutdownHost & other) const { return hash_ == other.hash_; } - bool operator!=(const ShutdownHost& other) const - { - return hash_ != other.hash_; - } + bool operator!=(const ShutdownHost & other) const { return hash_ != other.hash_; } - bool operator<(const ShutdownHost& other) const - { - return hash_ < other.hash_; - } + bool operator<(const ShutdownHost & other) const { return hash_ < other.hash_; } private: const std::string ip_; @@ -230,25 +186,21 @@ class ShutdownHost bool update_response() { - if (!is_available()) - { + if (!is_available()) { close_connection(); throw std::runtime_error("Lost connection"); } - if (!ssh_channel_is_open(channel_)) - { + if (!ssh_channel_is_open(channel_)) { throw std::runtime_error("Channel closed"); } - if (timeout_exceeded()) - { + if (timeout_exceeded()) { close_connection(); throw std::runtime_error("Timeout exceeded"); } - if ((nbytes_ = ssh_channel_read_nonblocking(channel_, buffer_, sizeof(buffer_), 0)) >= 0) - { + if ((nbytes_ = ssh_channel_read_nonblocking(channel_, buffer_, sizeof(buffer_), 0)) >= 0) { output_.append(buffer_, nbytes_); return true; } @@ -264,11 +216,10 @@ class ShutdownHost return elapsed > timeout_ && is_available(); } - void ssh_execute_command(const std::string& command) + void ssh_execute_command(const std::string & command) { session_ = ssh_new(); - if (session_ == NULL) - { + if (session_ == NULL) { throw std::runtime_error("Failed to open session"); }; @@ -277,15 +228,13 @@ class ShutdownHost ssh_options_set(session_, SSH_OPTIONS_PORT, &port_); ssh_options_set(session_, SSH_OPTIONS_LOG_VERBOSITY, &verbosity_); - if (ssh_connect(session_) != SSH_OK) - { + if (ssh_connect(session_) != SSH_OK) { std::string err = ssh_get_error(session_); ssh_free(session_); throw std::runtime_error("Error connecting to host: " + err); } - if (ssh_userauth_publickey_auto(session_, NULL, NULL) != SSH_AUTH_SUCCESS) - { + if (ssh_userauth_publickey_auto(session_, NULL, NULL) != SSH_AUTH_SUCCESS) { std::string err = ssh_get_error(session_); ssh_disconnect(session_); ssh_free(session_); @@ -293,16 +242,14 @@ class ShutdownHost } channel_ = ssh_channel_new(session_); - if (channel_ == NULL) - { + if (channel_ == NULL) { std::string err = ssh_get_error(session_); ssh_disconnect(session_); ssh_free(session_); throw std::runtime_error("Failed to create ssh channel: " + err); } - if (ssh_channel_open_session(channel_) != SSH_OK) - { + if (ssh_channel_open_session(channel_) != SSH_OK) { std::string err = ssh_get_error(session_); ssh_channel_free(channel_); ssh_disconnect(session_); @@ -310,8 +257,7 @@ class ShutdownHost throw std::runtime_error("Failed to open ssh channel: " + err); } - if (ssh_channel_request_exec(channel_, command.c_str()) != SSH_OK) - { + if (ssh_channel_request_exec(channel_, command.c_str()) != SSH_OK) { std::string err = ssh_get_error(session_); ssh_channel_close(channel_); ssh_channel_free(channel_); diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 7c9c5fd7e..35646b605 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -38,7 +38,8 @@ namespace panther_manager class ShutdownHosts : public BT::StatefulActionNode { public: - explicit ShutdownHosts(const std::string& name, const BT::NodeConfig& conf) : BT::StatefulActionNode(name, conf) + explicit ShutdownHosts(const std::string & name, const BT::NodeConfig & conf) + : BT::StatefulActionNode(name, conf) { // TODO: @delihus What is the name of ros::this_node? node_name_ = name; @@ -48,27 +49,20 @@ class ShutdownHosts : public BT::StatefulActionNode virtual ~ShutdownHosts() = default; // method to be implemented by user - virtual void update_hosts(std::vector>& hosts) = 0; + virtual void update_hosts(std::vector> & hosts) = 0; - // method that can be overriden by user + // method that can be overridden by user virtual BT::NodeStatus post_process() { // return success only when all hosts succeeded - if (failed_hosts_.size() == 0) - { + if (failed_hosts_.size() == 0) { return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; } - std::string get_node_name() const - { - return node_name_; - } - std::vector const get_failed_hosts() - { - return failed_hosts_; - } + std::string get_node_name() const { return node_name_; } + std::vector const get_failed_hosts() { return failed_hosts_; } private: std::size_t check_host_index_ = 0; @@ -84,8 +78,7 @@ class ShutdownHosts : public BT::StatefulActionNode { update_hosts(hosts_); remove_duplicate_hosts(hosts_); - if (hosts_.size() <= 0) - { + if (hosts_.size() <= 0) { RCLCPP_ERROR_STREAM(*logger_, "Hosts list is empty! Check configuration!"); return BT::NodeStatus::FAILURE; } @@ -96,13 +89,11 @@ class ShutdownHosts : public BT::StatefulActionNode BT::NodeStatus onRunning() { - if (hosts_to_check_.size() <= 0) - { + if (hosts_to_check_.size() <= 0) { return post_process(); } - if (check_host_index_ >= hosts_to_check_.size()) - { + if (check_host_index_ >= hosts_to_check_.size()) { check_host_index_ = 0; } @@ -110,10 +101,11 @@ class ShutdownHosts : public BT::StatefulActionNode auto host = hosts_[host_index]; host->call(); - switch (host->get_state()) - { + switch (host->get_state()) { case ShutdownHostState::RESPONSE_RECEIVED: - RCLCPP_INFO_STREAM(*logger_, "Device at: " << host->get_ip() << " response:\n" << host->get_response()); + RCLCPP_INFO_STREAM( + *logger_, "Device at: " << host->get_ip() << " response:\n" + << host->get_response()); check_host_index_++; break; @@ -125,15 +117,17 @@ class ShutdownHosts : public BT::StatefulActionNode break; case ShutdownHostState::FAILURE: - RCLCPP_WARN_STREAM(*logger_, - "Failed to shutdown device at: " << host->get_ip() << " Error: " << host->get_error()); + RCLCPP_WARN_STREAM( + *logger_, + "Failed to shutdown device at: " << host->get_ip() << " Error: " << host->get_error()); failed_hosts_.push_back(host_index); hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); break; case ShutdownHostState::SKIPPED: - RCLCPP_WARN_STREAM(*logger_, "Device at: " << host->get_ip() << " not available, skipping..."); + RCLCPP_WARN_STREAM( + *logger_, "Device at: " << host->get_ip() << " not available, skipping..."); skipped_hosts_.push_back(host_index); hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); @@ -147,33 +141,32 @@ class ShutdownHosts : public BT::StatefulActionNode return BT::NodeStatus::RUNNING; } - void remove_duplicate_hosts(std::vector>& hosts) + void remove_duplicate_hosts(std::vector> & hosts) { std::set seen; - hosts.erase(std::remove_if(hosts.begin(), hosts.end(), - [&](const std::shared_ptr& host) { - if (!seen.count(*host)) - { - seen.insert(*host); - return false; - } - else - { - RCLCPP_WARN_STREAM(*logger_, "Found duplicate host: " << host->get_ip() - << " Processing only the " - "first " - "occurrence."); - return true; - } - }), - hosts.end()); + hosts.erase( + std::remove_if( + hosts.begin(), hosts.end(), + [&](const std::shared_ptr & host) { + if (!seen.count(*host)) { + seen.insert(*host); + return false; + } else { + RCLCPP_WARN_STREAM( + *logger_, "Found duplicate host: " << host->get_ip() + << " Processing only the " + "first " + "occurrence."); + return true; + } + }), + hosts.end()); } void onHalted() { - for (auto& host : hosts_) - { + for (auto & host : hosts_) { host->close_connection(); } } diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 38f9ff266..e8cd5acaf 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -14,13 +14,14 @@ Jakub Delicat + behaviortree_cpp + iputils-ping libboost-dev + libssh-dev rclcpp rclcpp_action - behaviortree_cpp std_srvs - libssh-dev - iputils-ping + yaml-cpp ament_lint_auto ament_lint_common diff --git a/panther_manager/src/bt_ros2.cpp b/panther_manager/src/bt_ros2.cpp index c4769ade7..4b307759a 100644 --- a/panther_manager/src/bt_ros2.cpp +++ b/panther_manager/src/bt_ros2.cpp @@ -1,4 +1,18 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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 "behaviortree_ros2/bt_action_node.hpp" #include "behaviortree_ros2/bt_service_node.hpp" -#include "behaviortree_ros2/bt_topic_sub_node.hpp" #include "behaviortree_ros2/bt_topic_pub_node.hpp" +#include "behaviortree_ros2/bt_topic_sub_node.hpp" diff --git a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp b/panther_manager/src/plugins/action/call_set_bool_service_node.cpp index 70ee082f2..fea389148 100644 --- a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp +++ b/panther_manager/src/plugins/action/call_set_bool_service_node.cpp @@ -1,27 +1,41 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 namespace panther_manager { -bool CallSetBoolService::setRequest(typename Request::SharedPtr& request) +bool CallSetBoolService::setRequest(typename Request::SharedPtr & request) { - if (!getInput("data", request->data)) - { + if (!getInput("data", request->data)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [data]"); return false; } return true; } -BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::SharedPtr& response) +BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::SharedPtr & response) { - if (!response->success) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + if (!response->success) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG_STREAM(node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + RCLCPP_DEBUG_STREAM( + node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp b/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp index 487af8a49..3b27c98a2 100644 --- a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp +++ b/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp @@ -1,26 +1,38 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + namespace panther_manager { -bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr& request) +bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr & request) { unsigned animation_id; - if (!getInput("id", animation_id)) - { + if (!getInput("id", animation_id)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [id]"); return false; } request->animation.id = static_cast(animation_id); - if (!getInput("param", request->animation.param)) - { + if (!getInput("param", request->animation.param)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [param]"); return false; } - if (!getInput("repeating", request->repeating)) - { + if (!getInput("repeating", request->repeating)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [repeating]"); return false; } @@ -28,16 +40,18 @@ bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr& request return true; } -BT::NodeStatus CallSetLedAnimationService::onResponseReceived(const typename Response::SharedPtr& response) +BT::NodeStatus CallSetLedAnimationService::onResponseReceived( + const typename Response::SharedPtr & response) { - if (!response->success) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + if (!response->success) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG_STREAM(node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + RCLCPP_DEBUG_STREAM( + node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/call_trigger_service_node.cpp b/panther_manager/src/plugins/action/call_trigger_service_node.cpp index a09c1023d..530ceb279 100644 --- a/panther_manager/src/plugins/action/call_trigger_service_node.cpp +++ b/panther_manager/src/plugins/action/call_trigger_service_node.cpp @@ -1,22 +1,35 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + namespace panther_manager { -bool CallTriggerService::setRequest(typename Request::SharedPtr& /*request*/) -{ - return true; -} +bool CallTriggerService::setRequest(typename Request::SharedPtr & /*request*/) { return true; } -BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::SharedPtr& response) +BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::SharedPtr & response) { - if (!response->success) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + if (!response->success) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to call " << prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } - RCLCPP_DEBUG_STREAM(node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + RCLCPP_DEBUG_STREAM( + node_->get_logger(), + "Successfully called " << prev_service_name_ << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp index cb14f2b19..ea0b571c5 100644 --- a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 @@ -14,28 +28,24 @@ namespace panther_manager { -void ShutdownHostsFromFile::update_hosts(std::vector>& hosts) +void ShutdownHostsFromFile::update_hosts(std::vector> & hosts) { std::string shutdown_hosts_file; - if (!getInput("shutdown_hosts_file", shutdown_hosts_file) || shutdown_hosts_file == "") - { + if ( + !getInput("shutdown_hosts_file", shutdown_hosts_file) || + shutdown_hosts_file == "") { throw(BT::RuntimeError("[", name(), "] Failed to get input [shutdown_hosts_file]")); } YAML::Node shutdown_hosts; - try - { + try { shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); - } - catch (const YAML::Exception& e) - { + } catch (const YAML::Exception & e) { throw BT::RuntimeError("[" + name() + "] Error loading YAML file: " + e.what()); } - for (const auto& host : shutdown_hosts["hosts"]) - { - if (!host["ip"] || !host["username"]) - { + for (const auto & host : shutdown_hosts["hosts"]) { + if (!host["ip"] || !host["username"]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(get_node_name()), "Missing info for remote host!"); continue; } @@ -43,27 +53,24 @@ void ShutdownHostsFromFile::update_hosts(std::vector(); auto user = host["username"].as(); unsigned port = 22; - if (host["port"]) - { + if (host["port"]) { port = host["port"].as(); } std::string command = "sudo shutdown now"; - if (host["command"]) - { + if (host["command"]) { command = host["command"].as(); } float timeout = 5.0; - if (host["timeout"]) - { + if (host["timeout"]) { timeout = host["timeout"].as(); } bool ping_for_success = true; - if (host["ping_for_success"]) - { + if (host["ping_for_success"]) { ping_for_success = host["ping_for_success"].as(); } - hosts.push_back(std::make_shared(ip, user, port, command, timeout, ping_for_success)); + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); } } diff --git a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp b/panther_manager/src/plugins/action/shutdown_single_host_node.cpp index 0bdfdc6ac..5aa6f084d 100644 --- a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp +++ b/panther_manager/src/plugins/action/shutdown_single_host_node.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 @@ -54,4 +68,4 @@ void ShutdownSingleHost::update_hosts(std::vector> BT_REGISTER_NODES(factory) { factory.registerNodeType("ShutdownSingleHost"); -} \ No newline at end of file +} diff --git a/panther_manager/src/plugins/action/signal_shutdown_node.cpp b/panther_manager/src/plugins/action/signal_shutdown_node.cpp index 2968581e6..802624c55 100644 --- a/panther_manager/src/plugins/action/signal_shutdown_node.cpp +++ b/panther_manager/src/plugins/action/signal_shutdown_node.cpp @@ -1,7 +1,21 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include #include #include @@ -27,4 +41,4 @@ BT::NodeStatus SignalShutdown::tick() BT_REGISTER_NODES(factory) { factory.registerNodeType("SignalShutdown"); -} \ No newline at end of file +} diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index bd452b84e..3a31f3e24 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -18,23 +18,25 @@ #include #include +#include #include #include #include -#include #include #include #include -#include +#include #include +#include #include namespace panther_manager_plugin_test { -struct BehaviorTreePluginDescription{ +struct BehaviorTreePluginDescription +{ std::string service_name; std::map params; }; @@ -42,27 +44,30 @@ struct BehaviorTreePluginDescription{ class PantherManagerPluginTestUtils { public: + std::string BuildBehaviorTree( + const std::string & plugin_name, const std::map & service); - std::string BuildBehaviorTree(const std::string& plugin_name, - const std::map & service); - - BT::Tree& CreateTree(const std::string& plugin_name, - const std::map & service); + BT::Tree & CreateTree( + const std::string & plugin_name, const std::map & service); - BT::BehaviorTreeFactory& GetFactory(); + BT::BehaviorTreeFactory & GetFactory(); void Start(); void Stop(); void CreateSetBoolServiceServer( - std::function - service_callback); + std::function< + void(std_srvs::srv::SetBool::Request::SharedPtr, std_srvs::srv::SetBool::Response::SharedPtr)> + service_callback); void CreateTriggerServiceServer( - std::function - service_callback); - void CreateSetLEDAnimationServiceServer(std::function - service_callback); + std::function< + void(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr)> + service_callback); + void CreateSetLEDAnimationServiceServer( + std::function + service_callback); private: rclcpp::Node::SharedPtr bt_node_; @@ -79,13 +84,13 @@ class PantherManagerPluginTestUtils void spin_executor(); - std::string header_ = R"( + const std::string header_ = R"( )"; - std::string footer_ = R"( + const std::string footer_ = R"( diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index 8d397062c..e6313fffa 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -1,19 +1,31 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 namespace panther_manager_plugin_test { -std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& plugin_name, - const std::map & service) +std::string PantherManagerPluginTestUtils::BuildBehaviorTree( + const std::string & plugin_name, const std::map & service) { std::stringstream bt; bt << header_ << std::endl; bt << "\t\t\t\t<" << plugin_name << " "; - - for (auto const& [key, value] : service) - { + for (auto const & [key, value] : service) { bt << key << "=\"" << value << "\" "; } @@ -22,19 +34,15 @@ std::string PantherManagerPluginTestUtils::BuildBehaviorTree(const std::string& return bt.str(); } -BT::Tree& PantherManagerPluginTestUtils::CreateTree(const std::string& plugin_name, - const std::map & service) +BT::Tree & PantherManagerPluginTestUtils::CreateTree( + const std::string & plugin_name, const std::map & service) { auto xml_text = BuildBehaviorTree(plugin_name, service); - std::cout << xml_text; tree_ = factory_.createTreeFromText(xml_text); return tree_; } -BT::BehaviorTreeFactory& PantherManagerPluginTestUtils::GetFactory() -{ - return factory_; -} +BT::BehaviorTreeFactory & PantherManagerPluginTestUtils::GetFactory() { return factory_; } void PantherManagerPluginTestUtils::Start() { @@ -45,60 +53,64 @@ void PantherManagerPluginTestUtils::Start() factory_.registerNodeType("CallSetBoolService", params); factory_.registerNodeType("CallTriggerService", params); - factory_.registerNodeType("CallSetLedAnimationService", params); + factory_.registerNodeType( + "CallSetLedAnimationService", params); factory_.registerNodeType("SignalShutdown"); factory_.registerNodeType("ShutdownSingleHost"); + factory_.registerNodeType("ShutdownHostsFromFile"); } void PantherManagerPluginTestUtils::Stop() { bt_node_.reset(); rclcpp::shutdown(); - if (executor_thread_) - { + if (executor_thread_) { executor_.reset(); executor_thread_->join(); } } void PantherManagerPluginTestUtils::CreateSetBoolServiceServer( - std::function - service_callback) + std::function< + void(std_srvs::srv::SetBool::Request::SharedPtr, std_srvs::srv::SetBool::Response::SharedPtr)> + service_callback) { service_server_node_ = std::make_shared("test_set_bool_service"); - set_bool_server_ = service_server_node_->create_service("set_bool", service_callback); + set_bool_server_ = service_server_node_->create_service( + "set_bool", service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } void PantherManagerPluginTestUtils::CreateTriggerServiceServer( - std::function - service_callback) + std::function< + void(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr)> + service_callback) { service_server_node_ = std::make_shared("test_trigger_service"); - trigger_server_ = service_server_node_->create_service("trigger", service_callback); + trigger_server_ = service_server_node_->create_service( + "trigger", service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( - std::function - service_callback) + std::function + service_callback) { service_server_node_ = std::make_shared("test_set_led_animation_service"); set_led_animation_server_ = - service_server_node_->create_service("set_led_animation", service_callback); + service_server_node_->create_service( + "set_led_animation", service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::spin_executor() -{ - executor_->spin(); -} +void PantherManagerPluginTestUtils::spin_executor() { executor_->spin(); } } // namespace panther_manager_plugin_test diff --git a/panther_manager/test/plugins/test_set_bool_plugin.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp similarity index 64% rename from panther_manager/test/plugins/test_set_bool_plugin.cpp rename to panther_manager/test/plugins/test_call_set_bool_service_node.cpp index fd6ef05e5..5f2bfe774 100644 --- a/panther_manager/test/plugins/test_set_bool_plugin.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -15,45 +15,51 @@ #include #include +#include #include #include -#include #include #include -void ServiceFailedCallback(const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) +void ServiceFailedCallback( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) { response->message = "Failed callback pass!"; response->success = false; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); } -void ServiceSuccessCallbackCheckTrueValue(const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) +void ServiceSuccessCallbackCheckTrueValue( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); EXPECT_EQ(request->data, true); } -void ServiceSuccessCallbackCheckFalseValue(const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) +void ServiceSuccessCallbackCheckFalseValue( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); EXPECT_EQ(request->data, false); } TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) { - std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -64,7 +70,7 @@ TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) { - std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -74,11 +80,11 @@ TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plu TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) { - std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -88,11 +94,11 @@ TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_init TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) { - std::map service = { { "service_name", "set_bool" }, { "data", "true" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); @@ -104,11 +110,11 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) { - std::map service = { { "service_name", "set_bool" }, { "data", "false" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); @@ -120,11 +126,11 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) { - std::map service = { { "service_name", "set_bool" }, { "data", "false" } }; + std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.CreateTree("CallSetBoolService", service); test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); @@ -136,11 +142,12 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) TEST(TestCallSetBoolService, wrong_service_value_defined) { - std::map service = { { "service_name", "set_bool" }, { "data", "wrong_bool" } }; + std::map service = { + {"service_name", "set_bool"}, {"data", "wrong_bool"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.CreateTree("CallSetBoolService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -148,7 +155,7 @@ TEST(TestCallSetBoolService, wrong_service_value_defined) test_utils.Stop(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp similarity index 52% rename from panther_manager/test/plugins/test_set_led_animation_plugin.cpp rename to panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 881811bd1..42a8a9f4a 100644 --- a/panther_manager/test/plugins/test_set_led_animation_plugin.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -15,61 +15,68 @@ #include #include +#include #include #include -#include #include #include -void ServiceFailedCallback(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +void ServiceFailedCallback( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Failed callback pass!"; response->success = false; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); } -void ServiceSuccessCallbackCheckRepeatingTrueValue(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +void ServiceSuccessCallbackCheckRepeatingTrueValue( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); EXPECT_EQ(request->repeating, true); } void ServiceSuccessCallbackCheckRepeatingFalseValue( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); EXPECT_EQ(request->repeating, false); } -void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) +void ServiceSuccessCallbackCheckId5( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { response->message = "Successfully callback pass!"; response->success = true; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_set_led_animation_plugin"), + response->message << " success: " << response->success << " id: " << request->animation.id + << " param: " << request->animation.param + << " repeating: " << request->repeating); EXPECT_EQ(request->animation.id, 5u); } @@ -77,8 +84,7 @@ void ServiceSuccessCallbackCheckId5(const panther_msgs::srv::SetLEDAnimation::Re TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service_plugin) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -86,27 +92,29 @@ TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service test_utils.Stop(); } -TEST(TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) +TEST( + TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); + EXPECT_THROW( + { test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); test_utils.Stop(); } -TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_service_server_not_initialized) +TEST( + TestCallSetLedAnimationService, + wrong_call_set_led_animation_service_service_server_not_initialized) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -114,15 +122,16 @@ TEST(TestCallSetLedAnimationService, wrong_call_set_led_animation_service_servic test_utils.Stop(); } -TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_true_repeating_value) +TEST( + TestCallSetLedAnimationService, + good_set_led_animation_call_service_success_with_true_repeating_value) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); @@ -132,15 +141,16 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success test_utils.Stop(); } -TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_false_repeating_value) +TEST( + TestCallSetLedAnimationService, + good_set_led_animation_call_service_success_with_false_repeating_value) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "false" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); @@ -153,12 +163,11 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "5" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); @@ -171,12 +180,11 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); @@ -189,12 +197,11 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "0" }, { "param", "" }, { "repeating", "wrong_bool" } - }; + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "wrong_bool"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -205,12 +212,11 @@ TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) { std::map service = { - { "service_name", "set_led_animation" }, { "id", "-5" }, { "param", "" }, { "repeating", "true" } - }; + {"service_name", "set_led_animation"}, {"id", "-5"}, {"param", ""}, {"repeating", "true"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -218,7 +224,7 @@ TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) test_utils.Stop(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_manager/test/plugins/test_trigger_plugin.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp similarity index 75% rename from panther_manager/test/plugins/test_trigger_plugin.cpp rename to panther_manager/test/plugins/test_call_trigger_service_node.cpp index 7d94a9164..11f5e9f43 100644 --- a/panther_manager/test/plugins/test_trigger_plugin.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -13,19 +13,20 @@ // limitations under the License. #include -#include #include +#include +#include #include #include -#include #include #include -void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) +void ServiceFailedCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { [[maybe_unused]] request; response->message = "Failed callback pass!"; @@ -33,8 +34,9 @@ void ServiceFailedCallback(const std_srvs::srv::Trigger::Request::SharedPtr requ RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } -void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) +void ServiceSuccessCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { [[maybe_unused]] request; response->message = "Successfully callback pass!"; @@ -44,7 +46,7 @@ void ServiceSuccessCallback(const std_srvs::srv::Trigger::Request::SharedPtr req TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) { - std::map service = { { "service_name", "trigger" }}; + std::map service = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -55,7 +57,7 @@ TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) { - std::map service = { { "service_name", "trigger" }}; + std::map service = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -65,11 +67,11 @@ TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plug TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) { - std::map service = { { "service_name", "trigger" }}; + std::map service = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.CreateTree("CallTriggerService", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -79,11 +81,11 @@ TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initi TEST(TestCallTriggerService, good_trigger_call_service_success) { - std::map service = { { "service_name", "trigger" }}; + std::map service = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.CreateTree("CallTriggerService", service); test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); @@ -95,11 +97,11 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) TEST(TestCallTriggerService, wrong_trigger_call_service_failure) { - std::map service = { { "service_name", "trigger" }}; + std::map service = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.CreateTree("CallTriggerService", service); test_utils.CreateTriggerServiceServer(ServiceFailedCallback); @@ -109,7 +111,7 @@ TEST(TestCallTriggerService, wrong_trigger_call_service_failure) test_utils.Stop(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp new file mode 100644 index 000000000..275559879 --- /dev/null +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include + +#include +#include +#include + +#include + +#include + +TEST(TestShutdownHostsFromFile, good_loading_shutdown_hosts_from_file_plugin) +{ + const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + + ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownHostsFromFile", service); }); + test_utils.Stop(); +} + +TEST(TestShutdownHostsFromFile, wrong_plugin_name_loading_shutdown_hosts_from_file_plugin) +{ + const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) +{ + const std::string file_path = "/tmp/test_wrong_cannot_find_file_shutdown_hosts_from_file"; + const std::map service = {{"shutdown_hosts_file", file_path}}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto & tree = test_utils.CreateTree("ShutdownHostsFromFile", service); + + EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); + + test_utils.Stop(); +} + +TEST(TestShutdownHostsFromFile, good_shutdown_hosts_from_file) +{ + const std::string config_file_path = + "/tmp/test_panther_manager_good_shutdown_hosts_from_file_config"; + const std::string test_file_path = "/tmp/test_panther_manager_good_shutdown_hosts_from_file"; + std::filesystem::remove(test_file_path); + std::filesystem::remove(config_file_path); + + EXPECT_FALSE(std::filesystem::exists(test_file_path)); + EXPECT_FALSE(std::filesystem::exists(config_file_path)); + + YAML::Node yaml; + yaml["hosts"][0]["ip"] = "localhost"; + yaml["hosts"][0]["username"] = "husarion"; + yaml["hosts"][0]["port"] = 22; + yaml["hosts"][0]["command"] = "touch " + test_file_path; + yaml["hosts"][0]["timeout"] = 5.0; + yaml["hosts"][0]["ping_for_success"] = false; + std::fstream config_file; + YAML::Emitter emitter(config_file); + + config_file.open(config_file_path, std::ios::app); + emitter << yaml; + + config_file.close(); + + const std::map service = {{"shutdown_hosts_file", config_file_path}}; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + auto & tree = test_utils.CreateTree("ShutdownHostsFromFile", service); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + std::filesystem::remove(test_file_path); + std::filesystem::remove(config_file_path); + + test_utils.Stop(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp similarity index 74% rename from panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp rename to panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 54c190437..fb0d246f1 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_plugin.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include -#include -#include #include +#include +#include +#include #include #include -#include #include #include @@ -27,8 +27,8 @@ TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) { std::map service = { - { "command", "pwd" }, { "ip", "localhost" }, { "ping_for_success", "false" }, - { "port", "22" }, { "timeout", "5.0" }, { "user", "husarion" }, + {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, + {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); @@ -41,8 +41,8 @@ TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plugin) { std::map service = { - { "command", "pwd" }, { "ip", "localhost" }, { "ping_for_success", "false" }, - { "port", "22" }, { "timeout", "5.0" }, { "user", "husarion" }, + {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, + {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; @@ -58,13 +58,17 @@ TEST(TestShutdownSingleHost, good_touch_command) EXPECT_FALSE(std::filesystem::exists(file_path)); std::map service = { - { "command", "touch " + file_path }, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "husarion" }, + {"command", "touch " + file_path}, + {"ip", "localhost"}, + {"ping_for_success", "false"}, + {"port", "22"}, + {"timeout", "5.0"}, + {"user", "husarion"}, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); - auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -77,15 +81,19 @@ TEST(TestShutdownSingleHost, good_touch_command) TEST(TestShutdownSingleHost, wrong_command) { std::map service = { - { "command", "command_what_does_not_exists"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "husarion" }, + {"command", "command_what_does_not_exists"}, + {"ip", "localhost"}, + {"ping_for_success", "false"}, + {"port", "22"}, + {"timeout", "0.2"}, + {"user", "husarion"}, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); - auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); test_utils.Stop(); @@ -94,13 +102,17 @@ TEST(TestShutdownSingleHost, wrong_command) TEST(TestShutdownSingleHost, wrong_user) { std::map service = { - { "command", "echo Hello World!"}, { "ip", "localhost" }, { "ping_for_success", "false" }, { "port", "22" }, - { "timeout", "5.0" }, { "user", "user_what_does_not_exists" }, + {"command", "echo Hello World!"}, + {"ip", "localhost"}, + {"ping_for_success", "false"}, + {"port", "22"}, + {"timeout", "5.0"}, + {"user", "user_what_does_not_exists"}, }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("ShutdownSingleHost", service); - auto& tree = test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); @@ -108,7 +120,7 @@ TEST(TestShutdownSingleHost, wrong_user) test_utils.Stop(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp similarity index 86% rename from panther_manager/test/plugins/test_signal_shutdown_plugin.cpp rename to panther_manager/test/plugins/test_signal_shutdown_node.cpp index 294efe113..781185b73 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_plugin.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -13,19 +13,19 @@ // limitations under the License. #include -#include #include +#include +#include #include #include -#include #include #include TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) { - std::map service = { { "reason", "Test shutdown." } }; + std::map service = {{"reason", "Test shutdown."}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); test_utils.CreateTree("SignalShutdown", service); @@ -46,10 +46,10 @@ TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) TEST(TestSignalShutdown, good_check_reason_blackboard_value) { - std::map service = { { "reason", "Test shutdown." } }; + std::map service = {{"reason", "Test shutdown."}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("SignalShutdown", service); + auto & tree = test_utils.CreateTree("SignalShutdown", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -64,10 +64,10 @@ TEST(TestSignalShutdown, good_check_reason_blackboard_value) TEST(TestSignalShutdown, wrong_check_reason_blackboard_value) { - std::map service = { { "reason", "Test shutdown." } }; + std::map service = {{"reason", "Test shutdown."}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - auto& tree = test_utils.CreateTree("SignalShutdown", service); + auto & tree = test_utils.CreateTree("SignalShutdown", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -79,7 +79,7 @@ TEST(TestSignalShutdown, wrong_check_reason_blackboard_value) EXPECT_FALSE(got_value.second == "Wrong reason!"); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 75337adec6be8836ccf6dbd0d8dc9525ef0b0b62 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Feb 2024 21:01:37 +0000 Subject: [PATCH 18/35] typo Signed-off-by: Jakub Delicat --- panther_manager/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_manager/package.xml b/panther_manager/package.xml index e8cd5acaf..ebb245c49 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -12,7 +12,7 @@ https://github.com/husarion/panther_ros https://github.com/husarion/panther_ros/issues - Jakub Delicat + Jakub Delicat behaviortree_cpp iputils-ping From 0e210294d1bf258dd29d5bf1598ba97c836bd741 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 28 Feb 2024 15:51:17 +0000 Subject: [PATCH 19/35] Added TickAfterTimeout Signed-off-by: Jakub Delicat --- panther_gazebo/CMakeLists.txt | 8 - .../config/battery_plugin_config.yaml | 19 -- panther_gazebo/config/gz_bridge.yaml | 21 -- panther_gazebo/launch/simulation.launch.py | 242 ------------------ panther_manager/CMakeLists.txt | 4 + .../decorator/tick_after_timeout_node.hpp | 45 ++++ .../decorator/tick_after_timeout_node.cpp | 62 +++++ .../panther_manager_plugin_test_utils.hpp | 1 + .../src/panther_manager_plugin_test_utils.cpp | 1 + .../plugins/test_tick_after_timeout_node.cpp | 52 ++++ 10 files changed, 165 insertions(+), 290 deletions(-) delete mode 100644 panther_gazebo/CMakeLists.txt delete mode 100644 panther_gazebo/config/battery_plugin_config.yaml delete mode 100644 panther_gazebo/config/gz_bridge.yaml delete mode 100644 panther_gazebo/launch/simulation.launch.py create mode 100644 panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp create mode 100644 panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp create mode 100644 panther_manager/test/plugins/test_tick_after_timeout_node.cpp diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt deleted file mode 100644 index 0709e4501..000000000 --- a/panther_gazebo/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(panther_gazebo) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin_config.yaml deleted file mode 100644 index 665710fdc..000000000 --- a/panther_gazebo/config/battery_plugin_config.yaml +++ /dev/null @@ -1,19 +0,0 @@ -# Parameters for the Ignition LinearBatteryPlugin - -# Enables battery simulation. If set to true, the battery will discharge, -# and if it depletes completely, the robot will stop moving. -simulate_discharging: false - -# Sets the initial charge percentage of the battery. -initial_charge_percentage: 70.0 - -# Specifies the charging time for the battery in hours. -charging_time: 6.0 # [h] - -# Represents the power load during normal operation and is initially set to 120W. -# With the default power load of 120W, the robot can operate for up to 8 hours. -# When the 'simulate_discharging' parameter is set to false, this value defaults to 0. -# Users are encouraged to customize this value to match their specific needs. -# For more information on Panther power consumption, please refer to: -# https://husarion.com/manuals/panther/#battery--charging -power_load: 120.0 # [W] diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml deleted file mode 100644 index b66c67330..000000000 --- a/panther_gazebo/config/gz_bridge.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# For detailed information on configuring the parameter_bridge, please refer to: -# https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml - -- topic_name: "clock" - ros_type_name: "rosgraph_msgs/msg/Clock" - gz_type_name: "ignition.msgs.Clock" - direction: GZ_TO_ROS - -- ros_topic_name: "battery" - gz_topic_name: "model/panther/battery/panther_battery/state" - ros_type_name: "sensor_msgs/msg/BatteryState" - gz_type_name: "ignition.msgs.BatteryState" - subscriber_queue: 10 - publisher_queue: 10 - direction: GZ_TO_ROS - -# Topic for Twist data from Ignition teleopt widget -- topic_name: "cmd_vel" - ros_type_name: "geometry_msgs/msg/Twist" - gz_type_name: "ignition.msgs.Twist" - direction: GZ_TO_ROS diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py deleted file mode 100644 index 68bbe83c3..000000000 --- a/panther_gazebo/launch/simulation.launch.py +++ /dev/null @@ -1,242 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 Husarion sp. z o.o. -# -# 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. - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, - PythonExpression, -) -from launch_ros.actions import Node, SetParameter - - -def generate_launch_description(): - wheel_type = LaunchConfiguration("wheel_type") - declare_wheel_type_arg = DeclareLaunchArgument( - "wheel_type", - default_value="WH01", - description=( - "Specify the type of wheel. If you select a value from the provided options ('WH01'," - " 'WH02', 'WH04'), you can disregard the 'wheel_config_path' and" - " 'controller_config_path' parameters. If you have custom wheels, set this parameter" - " to 'CUSTOM' and provide the necessary configurations." - ), - choices=["WH01", "WH02", "WH04", "CUSTOM"], - ) - - wheel_config_path = LaunchConfiguration("wheel_config_path") - declare_wheel_config_path_arg = DeclareLaunchArgument( - "wheel_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_description"), - "config", - PythonExpression(["'", wheel_type, ".yaml'"]), - ] - ), - description=( - "Path to wheel configuration file. By default, it is located in " - "'panther_description/config/.yaml'. You can also specify the path " - "to your custom wheel configuration file here. " - ), - ) - - controller_config_path = LaunchConfiguration("controller_config_path") - declare_controller_config_path_arg = DeclareLaunchArgument( - "controller_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_controller"), - "config", - PythonExpression(["'", wheel_type, "_controller.yaml'"]), - ] - ), - description=( - "Path to controller configuration file. By default, it is located in" - " 'panther_controller/config/_controller.yaml'. You can also specify" - " the path to your custom controller configuration file here. " - ), - ) - - battery_config_path = LaunchConfiguration("battery_config_path") - declare_battery_config_path_arg = DeclareLaunchArgument( - "battery_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_gazebo"), - "config", - "battery_plugin_config.yaml", - ] - ), - description=( - "Path to the Ignition LinearBatteryPlugin configuration file. " - "This configuration is intended for use in simulations only." - ), - ) - - gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") - declare_gz_bridge_config_path_arg = DeclareLaunchArgument( - "gz_bridge_config_path", - default_value=PathJoinSubstitution( - [ - get_package_share_directory("panther_gazebo"), - "config", - "gz_bridge.yaml", - ] - ), - description="Path to the parameter_bridge configuration file", - ) - - world_cfg = LaunchConfiguration("world") - declare_world_arg = DeclareLaunchArgument( - "world", - default_value=[ - "-r ", - PathJoinSubstitution( - [ - get_package_share_directory("husarion_office_gz"), - "worlds", - "husarion_world.sdf", - ], - ), - ], - description="SDF world file", - ) - - pose_x = LaunchConfiguration("pose_x") - declare_pose_x_arg = DeclareLaunchArgument( - "pose_x", - default_value=["5.0"], - description="Initial robot position in the global 'x' axis.", - ) - - pose_y = LaunchConfiguration("pose_y") - declare_pose_y_arg = DeclareLaunchArgument( - "pose_y", - default_value=["-5.0"], - description="Initial robot position in the global 'y' axis.", - ) - - pose_z = LaunchConfiguration("pose_z") - declare_pose_z_arg = DeclareLaunchArgument( - "pose_z", - default_value=["0.2"], - description="Initial robot position in the global 'z' axis.", - ) - - rot_yaw = LaunchConfiguration("rot_yaw") - declare_rot_yaw_arg = DeclareLaunchArgument( - "rot_yaw", default_value=["0.0"], description="Initial robot orientation." - ) - - publish_robot_state = LaunchConfiguration("publish_robot_state") - declare_publish_robot_state_arg = DeclareLaunchArgument( - "publish_robot_state", - default_value="True", - description=( - "Whether to launch the robot_state_publisher node." - "When set to False, users should publish their own robot description." - ), - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={"gz_args": world_cfg}.items(), - ) - - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "panther", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - pose_x, - "-y", - pose_y, - "-z", - pose_z, - "-Y", - rot_yaw, - ], - output="screen", - ) - - gz_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_bridge", - parameters=[{"config_file": gz_bridge_config_path}], - output="screen", - ) - - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("panther_bringup"), - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "wheel_type": wheel_type, - "wheel_config_path": wheel_config_path, - "controller_config_path": controller_config_path, - "battery_config_path": battery_config_path, - "publish_robot_state": publish_robot_state, - "use_sim": "True", - "simulation_engine": "ignition-gazebo", - }.items(), - ) - - return LaunchDescription( - [ - declare_world_arg, - declare_pose_x_arg, - declare_pose_y_arg, - declare_pose_z_arg, - declare_rot_yaw_arg, - declare_wheel_type_arg, - declare_wheel_config_path_arg, - declare_controller_config_path_arg, - declare_battery_config_path_arg, - declare_gz_bridge_config_path_arg, - declare_publish_robot_state_arg, - # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - gz_bridge, - gz_spawn_entity, - bringup_launch, - ] - ) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index e5fd2f824..1480bd846 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -49,6 +49,10 @@ add_library(shutdown_hosts_from_file_node SHARED target_link_libraries(shutdown_hosts_from_file_node ssh yaml-cpp) list(APPEND plugin_libs shutdown_hosts_from_file_node) +add_library(tick_after_timeout_node SHARED + src/plugins/decorator/tick_after_timeout_node.cpp) +list(APPEND plugin_libs tick_after_timeout_node) + foreach(bt_node ${plugin_libs}) target_compile_definitions(${bt_node} PRIVATE BT_PLUGIN_EXPORT) ament_target_dependencies(${bt_node} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp new file mode 100644 index 000000000..c47835545 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ +#define PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ + +#include +#include + +#include +#include +#include + +namespace panther_manager +{ +class TickAfterTimeout : public BT::DecoratorNode +{ +public: + TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf); + + static BT::PortsList providedPorts() + { + return {BT::InputPort("timeout", "time in s to wait before ticking child again")}; + } + +private: + std::chrono::duration timeout_; + std::chrono::time_point last_success_time_; + + BT::NodeStatus tick() override; +}; +} // namespace panther_manager + +#endif // PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ diff --git a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp b/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp new file mode 100644 index 000000000..19ca4a004 --- /dev/null +++ b/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp @@ -0,0 +1,62 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +namespace panther_manager +{ + +TickAfterTimeout::TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf) +: BT::DecoratorNode(name, conf) +{ + last_success_time_ = std::chrono::steady_clock::now(); +} + +BT::NodeStatus TickAfterTimeout::tick() +{ + float timeout; + if (!getInput("timeout", timeout)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [timeout]")); + } + + timeout_ = std::chrono::duration(timeout); + + auto dt = std::chrono::duration_cast( + std::chrono::steady_clock::now() - last_success_time_); + + if (dt < timeout_) { + return BT::NodeStatus::SKIPPED; + } + + setStatus(BT::NodeStatus::RUNNING); + auto child_status = child()->executeTick(); + + if (child_status == BT::NodeStatus::SUCCESS) { + last_success_time_ = std::chrono::steady_clock::now(); + } + + if (child_status != BT::NodeStatus::RUNNING) { + resetChild(); + } + + return child_status; +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TickAfterTimeout"); +} diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 3a31f3e24..917ceb4f7 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index e6313fffa..c3a18ef7b 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -58,6 +58,7 @@ void PantherManagerPluginTestUtils::Start() factory_.registerNodeType("SignalShutdown"); factory_.registerNodeType("ShutdownSingleHost"); factory_.registerNodeType("ShutdownHostsFromFile"); + factory_.registerNodeType("TickAfterTimeout"); } void PantherManagerPluginTestUtils::Stop() diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp new file mode 100644 index 000000000..d64795516 --- /dev/null +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -0,0 +1,52 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +#include +#include +#include + +#include +#include + +TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) +{ + std::map service = {{"timeout", "0.0"}}; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + test_utils.CreateTree("TickAfterTimeout", service); + + ASSERT_NO_THROW({ test_utils.CreateTree("TickAfterTimeout", service); }); + test_utils.Stop(); +} + +TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) +{ + std::map service = {}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW({ test_utils.CreateTree("WrongTickAfterTimeout", service); }, BT::RuntimeError); + test_utils.Stop(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 428d201818fa5d69ed043aeefe2c03bbba6a528b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 6 Mar 2024 07:30:05 +0000 Subject: [PATCH 20/35] reverted panther_gazebo Signed-off-by: Jakub Delicat --- panther_gazebo/CMakeLists.txt | 8 + panther_gazebo/README.md | 2 + .../config/battery_plugin_config.yaml | 19 ++ panther_gazebo/config/gz_bridge.yaml | 21 ++ panther_gazebo/launch/simulation.launch.py | 242 ++++++++++++++++++ panther_gazebo/package.xml | 2 +- 6 files changed, 293 insertions(+), 1 deletion(-) create mode 100644 panther_gazebo/CMakeLists.txt create mode 100644 panther_gazebo/config/battery_plugin_config.yaml create mode 100644 panther_gazebo/config/gz_bridge.yaml create mode 100644 panther_gazebo/launch/simulation.launch.py diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt new file mode 100644 index 000000000..0709e4501 --- /dev/null +++ b/panther_gazebo/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.10.2) +project(panther_gazebo) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 14409db6d..541a7b4d8 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -60,6 +60,7 @@ The NavSat system is utilized to publish the Panther robot's position within the The NavSat sensors requires the spherical coordinates of the world origin to be configured. This configuration can be accomplished, for instance, by employing the `` tag within the world's SDF or by utilizing the Ignition `/world/world_name/set_spherical_coordinates` service. To obtain GPS data in Ignition, follow these steps: + - Include the [external_antenna](../panther_description/urdf/components/external_antenna.urdf.xacro) macro in your robot model by adding the following lines to your [panther.urdf.xacro](../panther_description/urdf/panther.urdf.xacro) file within the `` tag: ```xml @@ -71,6 +72,7 @@ To obtain GPS data in Ignition, follow these steps: ``` - Add the following tag to your world's SDF file and specify this file using the `world` parameter (the default `husarion_world.sdf` file already includes this tag): + ```xml EARTH_WGS84 diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin_config.yaml new file mode 100644 index 000000000..665710fdc --- /dev/null +++ b/panther_gazebo/config/battery_plugin_config.yaml @@ -0,0 +1,19 @@ +# Parameters for the Ignition LinearBatteryPlugin + +# Enables battery simulation. If set to true, the battery will discharge, +# and if it depletes completely, the robot will stop moving. +simulate_discharging: false + +# Sets the initial charge percentage of the battery. +initial_charge_percentage: 70.0 + +# Specifies the charging time for the battery in hours. +charging_time: 6.0 # [h] + +# Represents the power load during normal operation and is initially set to 120W. +# With the default power load of 120W, the robot can operate for up to 8 hours. +# When the 'simulate_discharging' parameter is set to false, this value defaults to 0. +# Users are encouraged to customize this value to match their specific needs. +# For more information on Panther power consumption, please refer to: +# https://husarion.com/manuals/panther/#battery--charging +power_load: 120.0 # [W] diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml new file mode 100644 index 000000000..b66c67330 --- /dev/null +++ b/panther_gazebo/config/gz_bridge.yaml @@ -0,0 +1,21 @@ +# For detailed information on configuring the parameter_bridge, please refer to: +# https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml + +- topic_name: "clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "battery" + gz_topic_name: "model/panther/battery/panther_battery/state" + ros_type_name: "sensor_msgs/msg/BatteryState" + gz_type_name: "ignition.msgs.BatteryState" + subscriber_queue: 10 + publisher_queue: 10 + direction: GZ_TO_ROS + +# Topic for Twist data from Ignition teleopt widget +- topic_name: "cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "ignition.msgs.Twist" + direction: GZ_TO_ROS diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py new file mode 100644 index 000000000..68bbe83c3 --- /dev/null +++ b/panther_gazebo/launch/simulation.launch.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +# Copyright 2023 Husarion sp. z o.o. +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node, SetParameter + + +def generate_launch_description(): + wheel_type = LaunchConfiguration("wheel_type") + declare_wheel_type_arg = DeclareLaunchArgument( + "wheel_type", + default_value="WH01", + description=( + "Specify the type of wheel. If you select a value from the provided options ('WH01'," + " 'WH02', 'WH04'), you can disregard the 'wheel_config_path' and" + " 'controller_config_path' parameters. If you have custom wheels, set this parameter" + " to 'CUSTOM' and provide the necessary configurations." + ), + choices=["WH01", "WH02", "WH04", "CUSTOM"], + ) + + wheel_config_path = LaunchConfiguration("wheel_config_path") + declare_wheel_config_path_arg = DeclareLaunchArgument( + "wheel_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_description"), + "config", + PythonExpression(["'", wheel_type, ".yaml'"]), + ] + ), + description=( + "Path to wheel configuration file. By default, it is located in " + "'panther_description/config/.yaml'. You can also specify the path " + "to your custom wheel configuration file here. " + ), + ) + + controller_config_path = LaunchConfiguration("controller_config_path") + declare_controller_config_path_arg = DeclareLaunchArgument( + "controller_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_controller"), + "config", + PythonExpression(["'", wheel_type, "_controller.yaml'"]), + ] + ), + description=( + "Path to controller configuration file. By default, it is located in" + " 'panther_controller/config/_controller.yaml'. You can also specify" + " the path to your custom controller configuration file here. " + ), + ) + + battery_config_path = LaunchConfiguration("battery_config_path") + declare_battery_config_path_arg = DeclareLaunchArgument( + "battery_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_gazebo"), + "config", + "battery_plugin_config.yaml", + ] + ), + description=( + "Path to the Ignition LinearBatteryPlugin configuration file. " + "This configuration is intended for use in simulations only." + ), + ) + + gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") + declare_gz_bridge_config_path_arg = DeclareLaunchArgument( + "gz_bridge_config_path", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_gazebo"), + "config", + "gz_bridge.yaml", + ] + ), + description="Path to the parameter_bridge configuration file", + ) + + world_cfg = LaunchConfiguration("world") + declare_world_arg = DeclareLaunchArgument( + "world", + default_value=[ + "-r ", + PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "husarion_world.sdf", + ], + ), + ], + description="SDF world file", + ) + + pose_x = LaunchConfiguration("pose_x") + declare_pose_x_arg = DeclareLaunchArgument( + "pose_x", + default_value=["5.0"], + description="Initial robot position in the global 'x' axis.", + ) + + pose_y = LaunchConfiguration("pose_y") + declare_pose_y_arg = DeclareLaunchArgument( + "pose_y", + default_value=["-5.0"], + description="Initial robot position in the global 'y' axis.", + ) + + pose_z = LaunchConfiguration("pose_z") + declare_pose_z_arg = DeclareLaunchArgument( + "pose_z", + default_value=["0.2"], + description="Initial robot position in the global 'z' axis.", + ) + + rot_yaw = LaunchConfiguration("rot_yaw") + declare_rot_yaw_arg = DeclareLaunchArgument( + "rot_yaw", default_value=["0.0"], description="Initial robot orientation." + ) + + publish_robot_state = LaunchConfiguration("publish_robot_state") + declare_publish_robot_state_arg = DeclareLaunchArgument( + "publish_robot_state", + default_value="True", + description=( + "Whether to launch the robot_state_publisher node." + "When set to False, users should publish their own robot description." + ), + ) + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={"gz_args": world_cfg}.items(), + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "panther", + "-allow_renaming", + "true", + "-topic", + "robot_description", + "-x", + pose_x, + "-y", + pose_y, + "-z", + pose_z, + "-Y", + rot_yaw, + ], + output="screen", + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_bridge", + parameters=[{"config_file": gz_bridge_config_path}], + output="screen", + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("panther_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "wheel_type": wheel_type, + "wheel_config_path": wheel_config_path, + "controller_config_path": controller_config_path, + "battery_config_path": battery_config_path, + "publish_robot_state": publish_robot_state, + "use_sim": "True", + "simulation_engine": "ignition-gazebo", + }.items(), + ) + + return LaunchDescription( + [ + declare_world_arg, + declare_pose_x_arg, + declare_pose_y_arg, + declare_pose_z_arg, + declare_rot_yaw_arg, + declare_wheel_type_arg, + declare_wheel_config_path_arg, + declare_controller_config_path_arg, + declare_battery_config_path_arg, + declare_gz_bridge_config_path_arg, + declare_publish_robot_state_arg, + # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + gz_sim, + gz_bridge, + gz_spawn_entity, + bringup_launch, + ] + ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 7b88a3522..afdb95efd 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -3,7 +3,7 @@ panther_gazebo 0.0.1 - The panther_gazebo package + The panther_description package Husarion Apache License 2.0 From 32ec819e3934ec9c4394c9c6314ed36a766cad57 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 6 Mar 2024 15:44:30 +0000 Subject: [PATCH 21/35] added tests for tick_after_timeout Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 7 +- .../panther_manager_plugin_test_utils.hpp | 8 ++- .../src/panther_manager_plugin_test_utils.cpp | 20 ++++-- .../test_shutdown_single_host_node.cpp | 1 - .../plugins/test_signal_shutdown_node.cpp | 1 - .../plugins/test_tick_after_timeout_node.cpp | 64 +++++++++++++++++-- 6 files changed, 84 insertions(+), 17 deletions(-) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 1480bd846..7a0abe479 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -73,7 +73,8 @@ if(BUILD_TESTING) call_set_led_animation_service_node signal_shutdown_node shutdown_single_host_node - shutdown_hosts_from_file_node) + shutdown_hosts_from_file_node + tick_after_timeout_node) ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils ${PACKAGE_INCLUDE_DEPENDS}) @@ -103,6 +104,10 @@ if(BUILD_TESTING) test/plugins/test_shutdown_hosts_from_file_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node) + ament_add_gtest(${PROJECT_NAME}_test_tick_after_timeout_node + test/plugins/test_tick_after_timeout_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_tick_after_timeout_node) + foreach(bt_node_test ${plugin_tests}) target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp index 917ceb4f7..598deeb2a 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp @@ -38,7 +38,7 @@ namespace panther_manager_plugin_test struct BehaviorTreePluginDescription { - std::string service_name; + std::string name; std::map params; }; @@ -46,10 +46,12 @@ class PantherManagerPluginTestUtils { public: std::string BuildBehaviorTree( - const std::string & plugin_name, const std::map & service); + const std::string & plugin_name, const std::map & service, + double tick_after_timeout); BT::Tree & CreateTree( - const std::string & plugin_name, const std::map & service); + const std::string & plugin_name, const std::map & service, + double tick_after_timeout = std::numeric_limits::quiet_NaN()); BT::BehaviorTreeFactory & GetFactory(); diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp index c3a18ef7b..1333ab475 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp @@ -18,26 +18,38 @@ namespace panther_manager_plugin_test { std::string PantherManagerPluginTestUtils::BuildBehaviorTree( - const std::string & plugin_name, const std::map & service) + const std::string & plugin_name, const std::map & service, + double tick_after_timeout) { std::stringstream bt; bt << header_ << std::endl; + if (not std::isnan(tick_after_timeout)) { + bt << "\t\t\t" << std::endl; + } + bt << "\t\t\t\t<" << plugin_name << " "; for (auto const & [key, value] : service) { bt << key << "=\"" << value << "\" "; } - bt << " />" << std::endl << footer_; + bt << " />" << std::endl; + + if (not std::isnan(tick_after_timeout)) { + bt << "\t\t\t" << std::endl; + } + + bt << footer_; return bt.str(); } BT::Tree & PantherManagerPluginTestUtils::CreateTree( - const std::string & plugin_name, const std::map & service) + const std::string & plugin_name, const std::map & service, + double tick_after_timeout) { - auto xml_text = BuildBehaviorTree(plugin_name, service); + auto xml_text = BuildBehaviorTree(plugin_name, service, tick_after_timeout); tree_ = factory_.createTreeFromText(xml_text); return tree_; } diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index fb0d246f1..918712f57 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -32,7 +32,6 @@ TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) }; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - test_utils.CreateTree("ShutdownSingleHost", service); ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownSingleHost", service); }); test_utils.Stop(); diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp index 781185b73..3a71478c8 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -28,7 +28,6 @@ TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) std::map service = {{"reason", "Test shutdown."}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - test_utils.CreateTree("SignalShutdown", service); ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); test_utils.Stop(); diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index d64795516..fa1a5f422 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -20,27 +20,77 @@ #include #include -#include +#include +#include #include +inline static std::size_t counter = 0; + +void CounterIncrease( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + [[maybe_unused]] request; + response->message = "Successfully increased!"; + response->success = true; + ++counter; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("test_tick_after_timeout_plugin"), + response->message << " Counter value: " << counter); +} + TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) { - std::map service = {{"timeout", "0.0"}}; + std::map trigger_node = {{"service_name", "trigger"}}; + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - test_utils.CreateTree("TickAfterTimeout", service); - - ASSERT_NO_THROW({ test_utils.CreateTree("TickAfterTimeout", service); }); + test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); + ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); }); test_utils.Stop(); } TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) { - std::map service = {}; + std::map trigger_node = {{"service_name", "trigger"}}; + + panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; + test_utils.Start(); + EXPECT_THROW( + { test_utils.CreateTree("WrongTriggerService", trigger_node, 0.1); }, BT::RuntimeError); + test_utils.Stop(); +} + +TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) +{ + std::map trigger_node = {{"service_name", "trigger"}}; panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; test_utils.Start(); - EXPECT_THROW({ test_utils.CreateTree("WrongTickAfterTimeout", service); }, BT::RuntimeError); + auto & tree = test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); + test_utils.CreateTriggerServiceServer(CounterIncrease); + + EXPECT_EQ(counter, 0); + + auto status = BT::NodeStatus::IDLE; + + for (std::size_t i = 2; i < 5; ++i) { + auto start = std::chrono::high_resolution_clock::now(); + int64_t duration; + while ((status = tree.tickOnce()) != BT::NodeStatus::FAILURE) { + auto end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end - start).count(); + EXPECT_LT(duration, 101); + if (counter == i and status == BT::NodeStatus::SUCCESS) { + break; + } + } + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + EXPECT_EQ(duration, 100); + } + EXPECT_EQ(counter, 4); + test_utils.Stop(); } From 3f58a584df229c76563311418b209fe5ad03fcf2 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Mar 2024 09:55:17 +0000 Subject: [PATCH 22/35] Added package suggestions without tests --- panther/panther_hardware.repos | 4 + panther_manager/CMakeLists.txt | 21 +- .../behaviortree_ros2/bt_action_node.hpp | 405 ------------------ .../behaviortree_ros2/bt_service_node.hpp | 304 ------------- .../behaviortree_ros2/bt_topic_pub_node.hpp | 168 -------- .../behaviortree_ros2/bt_topic_sub_node.hpp | 264 ------------ .../include/behaviortree_ros2/plugins.hpp | 59 --- .../behaviortree_ros2/ros_node_params.hpp | 45 -- .../action/call_set_bool_service_node.hpp | 3 + .../call_set_led_animation_service_node.hpp | 5 +- .../action/call_trigger_service_node.hpp | 5 +- .../panther_manager/plugins/shutdown_host.hpp | 8 +- .../plugins/shutdown_hosts_node.hpp | 66 ++- panther_manager/package.xml | 4 +- .../action/call_set_bool_service_node.cpp | 9 +- .../call_set_led_animation_service_node.cpp | 19 +- .../action/call_trigger_service_node.cpp | 11 +- .../action/shutdown_hosts_from_file_node.cpp | 6 +- .../action/shutdown_single_host_node.cpp | 12 +- .../plugins/action/signal_shutdown_node.cpp | 5 +- .../decorator/tick_after_timeout_node.cpp | 12 +- panther_manager/src/bt_ros2.cpp | 18 - 22 files changed, 103 insertions(+), 1350 deletions(-) delete mode 100644 panther_manager/include/behaviortree_ros2/bt_action_node.hpp delete mode 100644 panther_manager/include/behaviortree_ros2/bt_service_node.hpp delete mode 100644 panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp delete mode 100644 panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp delete mode 100644 panther_manager/include/behaviortree_ros2/plugins.hpp delete mode 100644 panther_manager/include/behaviortree_ros2/ros_node_params.hpp rename panther_manager/{src => }/plugins/action/call_set_bool_service_node.cpp (80%) rename panther_manager/{src => }/plugins/action/call_set_led_animation_service_node.cpp (71%) rename panther_manager/{src => }/plugins/action/call_trigger_service_node.cpp (76%) rename panther_manager/{src => }/plugins/action/shutdown_hosts_from_file_node.cpp (89%) rename panther_manager/{src => }/plugins/action/shutdown_single_host_node.cpp (78%) rename panther_manager/{src => }/plugins/action/signal_shutdown_node.cpp (85%) rename panther_manager/{src => }/plugins/decorator/tick_after_timeout_node.cpp (82%) delete mode 100644 panther_manager/src/bt_ros2.cpp diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index ae43bf638..4f1a83692 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/husarion/husarion_controllers version: main + behaviortree_ros2: + type: git + url: https://github.com/BehaviorTree/BehaviorTree.ROS2 + version: humble diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 7a0abe479..ba591754a 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -14,43 +14,42 @@ set(PACKAGE_INCLUDE_DEPENDS std_srvs panther_msgs libssh - yaml-cpp) + yaml-cpp + behaviortree_ros2) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() include_directories(include) -add_library(behaviortree_ros2 src/bt_ros2.cpp) add_library(call_set_bool_service_node SHARED - src/plugins/action/call_set_bool_service_node.cpp) + plugins/action/call_set_bool_service_node.cpp) list(APPEND plugin_libs call_set_bool_service_node) add_library(call_trigger_service_node SHARED - src/plugins/action/call_trigger_service_node.cpp) + plugins/action/call_trigger_service_node.cpp) list(APPEND plugin_libs call_trigger_service_node) add_library(call_set_led_animation_service_node SHARED - src/plugins/action/call_set_led_animation_service_node.cpp) + plugins/action/call_set_led_animation_service_node.cpp) list(APPEND plugin_libs call_set_led_animation_service_node) -add_library(signal_shutdown_node SHARED - src/plugins/action/signal_shutdown_node.cpp) +add_library(signal_shutdown_node SHARED plugins/action/signal_shutdown_node.cpp) list(APPEND plugin_libs signal_shutdown_node) add_library(shutdown_single_host_node SHARED - src/plugins/action/shutdown_single_host_node.cpp) + plugins/action/shutdown_single_host_node.cpp) target_link_libraries(shutdown_single_host_node ssh) list(APPEND plugin_libs shutdown_single_host_node) add_library(shutdown_hosts_from_file_node SHARED - src/plugins/action/shutdown_hosts_from_file_node.cpp) + plugins/action/shutdown_hosts_from_file_node.cpp) target_link_libraries(shutdown_hosts_from_file_node ssh yaml-cpp) list(APPEND plugin_libs shutdown_hosts_from_file_node) add_library(tick_after_timeout_node SHARED - src/plugins/decorator/tick_after_timeout_node.cpp) + plugins/decorator/tick_after_timeout_node.cpp) list(APPEND plugin_libs tick_after_timeout_node) foreach(bt_node ${plugin_libs}) @@ -116,8 +115,6 @@ endif() install(TARGETS DESTINATION lib/${PROJECT_NAME}) -ament_target_dependencies(behaviortree_ros2 ${PACKAGE_INCLUDE_DEPENDS}) - ament_export_include_directories(include) ament_export_dependencies(${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/include/behaviortree_ros2/bt_action_node.hpp b/panther_manager/include/behaviortree_ros2/bt_action_node.hpp deleted file mode 100644 index 274cbae0f..000000000 --- a/panther_manager/include/behaviortree_ros2/bt_action_node.hpp +++ /dev/null @@ -1,405 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// Copyright (c) 2023 Davide Faconti -// -// 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. - -#pragma once - -#include -#include -#include -#include -#include "behaviortree_cpp/action_node.h" -#include "behaviortree_cpp/bt_factory.h" -#include "rclcpp_action/rclcpp_action.hpp" - -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ - -enum ActionNodeErrorCode { - SERVER_UNREACHABLE, - SEND_GOAL_TIMEOUT, - GOAL_REJECTED_BY_SERVER, - ACTION_ABORTED, - ACTION_CANCELLED, - INVALID_GOAL -}; - -inline const char * toStr(const ActionNodeErrorCode & err) -{ - switch (err) { - case SERVER_UNREACHABLE: - return "SERVER_UNREACHABLE"; - case SEND_GOAL_TIMEOUT: - return "SEND_GOAL_TIMEOUT"; - case GOAL_REJECTED_BY_SERVER: - return "GOAL_REJECTED_BY_SERVER"; - case ACTION_ABORTED: - return "ACTION_ABORTED"; - case ACTION_CANCELLED: - return "ACTION_CANCELLED"; - case INVALID_GOAL: - return "INVALID_GOAL"; - } - return nullptr; -} - -/** - * @brief Abstract class to wrap rclcpp_action::Client<> - * - * For instance, given the type AddTwoInts described in this tutorial: - * https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html - * - * the corresponding wrapper would be: - * - * class FibonacciNode: public RosActionNode - * - * RosActionNode will try to be non-blocking for the entire duration of the call. - * The derived class must reimplement the virtual methods as described below. - * - * The name of the action will be determined as follows: - * - * 1. If a value is passes in the InputPort "action_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosActionNode : public BT::ActionNodeBase -{ -public: - // Type definitions - using ActionType = ActionT; - using ActionClient = typename rclcpp_action::Client; - using ActionClientPtr = std::shared_ptr; - using Goal = typename ActionT::Goal; - using GoalHandle = typename rclcpp_action::ClientGoalHandle; - using WrappedResult = typename rclcpp_action::ClientGoalHandle::WrappedResult; - using Feedback = typename ActionT::Feedback; - - /** To register this class into the factory, use: - * - * factory.registerNodeType<>(node_name, params); - * - */ - explicit RosActionNode( - const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); - - virtual ~RosActionNode() = default; - - /** - * @brief Any subclass of RosActionNode that has ports must implement a - * providedPorts method and call providedBasicPorts in it. - * - * @param addition Additional ports to add to BT port list - * @return PortsList containing basic ports along with node-specific ports - */ - static PortsList providedBasicPorts(PortsList addition) - { - PortsList basic = { - InputPort("action_name", "__default__placeholder__", "Action server name")}; - basic.insert(addition.begin(), addition.end()); - return basic; - } - - /** - * @brief Creates list of BT ports - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedPorts() { return providedBasicPorts({}); } - - /// @brief Callback executed when the node is halted. Note that cancelGoal() - /// is done automatically. - virtual void onHalt() {} - - /** setGoal s a callback that allows the user to set - * the goal message (ActionT::Goal). - * - * @param goal the goal to be sent to the action server. - * - * @return false if the request should not be sent. In that case, - * RosActionNode::onFailure(INVALID_GOAL) will be called. - */ - virtual bool setGoal(Goal & goal) = 0; - - /** Callback invoked when the result is received by the server. - * It is up to the user to define if the action returns SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onResultReceived(const WrappedResult & result) = 0; - - /** Callback invoked when the feedback is received. - * It generally returns RUNNING, but the user can also use this callback to cancel the - * current action and return SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFeedback(const std::shared_ptr /*feedback*/) - { - return NodeStatus::RUNNING; - } - - /** Callback invoked when something goes wrong. - * It must return either SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) { return NodeStatus::FAILURE; } - - /// Method used to send a request to the Action server to cancel the current goal - void cancelGoal(); - - /// The default halt() implementation will call cancelGoal if necessary. - void halt() override final; - - NodeStatus tick() override final; - -protected: - std::shared_ptr node_; - std::string prev_action_name_; - bool action_name_may_change_ = false; - const std::chrono::milliseconds server_timeout_; - const std::chrono::milliseconds wait_for_server_timeout_; - -private: - ActionClientPtr action_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - - std::shared_future future_goal_handle_; - typename GoalHandle::SharedPtr goal_handle_; - - rclcpp::Time time_goal_sent_; - NodeStatus on_feedback_state_change_; - bool goal_received_; - WrappedResult result_; - - bool createClient(const std::string & action_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template -inline RosActionNode::RosActionNode( - const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) -: BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - server_timeout_(params.server_timeout), - wait_for_server_timeout_(params.wait_for_server_timeout) -{ - // Three cases: - // - we use the default action_name in RosNodeParams when port is empty - // - we use the action_name in the port and it is a static string. - // - we use the action_name in the port and it is blackboard entry. - - // check port remapping - auto portIt = config().input_ports.find("action_name"); - if (portIt != config().input_ports.end()) { - const std::string & bb_action_name = portIt->second; - - if (bb_action_name.empty() || bb_action_name == "__default__placeholder__") { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } else { - createClient(params.default_port_value); - } - } else if (!isBlackboardPointer(bb_action_name)) { - // If the content of the port "action_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_action_name); - } else { - action_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } - } else { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } else { - createClient(params.default_port_value); - } - } -} - -template -inline bool RosActionNode::createClient(const std::string & action_name) -{ - if (action_name.empty()) { - throw RuntimeError("action_name is empty"); - } - - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); - - prev_action_name_ = action_name; - - bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); - if (!found) { - RCLCPP_ERROR( - node_->get_logger(), "%s: Action server with name '%s' is not reachable.", name().c_str(), - prev_action_name_.c_str()); - } - return found; -} - -template -inline NodeStatus RosActionNode::tick() -{ - // First, check if the action_client_ is valid and that the name of the - // action_name in the port didn't change. - // otherwise, create a new client - if (!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) { - std::string action_name; - getInput("action_name", action_name); - if (prev_action_name_ != action_name) { - createClient(action_name); - } - } - - //------------------------------------------ - auto CheckStatus = [](NodeStatus status) { - if (!isStatusCompleted(status)) { - throw std::logic_error("RosActionNode: the callback must return either SUCCESS of FAILURE"); - } - return status; - }; - - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) { - setStatus(NodeStatus::RUNNING); - - goal_received_ = false; - future_goal_handle_ = {}; - on_feedback_state_change_ = NodeStatus::RUNNING; - result_ = {}; - - Goal goal; - - if (!setGoal(goal)) { - return CheckStatus(onFailure(INVALID_GOAL)); - } - - typename ActionClient::SendGoalOptions goal_options; - - //-------------------- - goal_options.feedback_callback = - [this](typename GoalHandle::SharedPtr, const std::shared_ptr feedback) { - on_feedback_state_change_ = onFeedback(feedback); - if (on_feedback_state_change_ == NodeStatus::IDLE) { - throw std::logic_error("onFeedback must not return IDLE"); - } - emitWakeUpSignal(); - }; - //-------------------- - goal_options.result_callback = [this](const WrappedResult & result) { - if (goal_handle_->get_goal_id() == result.goal_id) { - RCLCPP_DEBUG(node_->get_logger(), "result_callback"); - result_ = result; - emitWakeUpSignal(); - } - }; - //-------------------- - goal_options.goal_response_callback = - [this](typename GoalHandle::SharedPtr const future_handle) { - auto goal_handle_ = future_handle.get(); - if (!goal_handle_) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - //-------------------- - - future_goal_handle_ = action_client_->async_send_goal(goal, goal_options); - time_goal_sent_ = node_->now(); - - return NodeStatus::RUNNING; - } - - if (status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); - - // FIRST case: check if the goal request has a timeout - if (!goal_received_) { - auto nodelay = std::chrono::milliseconds(0); - auto timeout = rclcpp::Duration::from_seconds(double(server_timeout_.count()) / 1000); - - auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - if ((node_->now() - time_goal_sent_) > timeout) { - return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); - } else { - return NodeStatus::RUNNING; - } - } else { - goal_received_ = true; - goal_handle_ = future_goal_handle_.get(); - future_goal_handle_ = {}; - - if (!goal_handle_) { - return CheckStatus(onFailure(GOAL_REJECTED_BY_SERVER)); - } - } - } - - // SECOND case: onFeedback requested a stop - if (on_feedback_state_change_ != NodeStatus::RUNNING) { - cancelGoal(); - return on_feedback_state_change_; - } - // THIRD case: result received, requested a stop - if (result_.code != rclcpp_action::ResultCode::UNKNOWN) { - if (result_.code == rclcpp_action::ResultCode::ABORTED) { - return CheckStatus(onFailure(ACTION_ABORTED)); - } else if (result_.code == rclcpp_action::ResultCode::CANCELED) { - return CheckStatus(onFailure(ACTION_CANCELLED)); - } else { - return CheckStatus(onResultReceived(result_)); - } - } - } - return NodeStatus::RUNNING; -} - -template -inline void RosActionNode::halt() -{ - if (status() == BT::NodeStatus::RUNNING) { - cancelGoal(); - onHalt(); - } -} - -template -inline void RosActionNode::cancelGoal() -{ - auto future_result = action_client_->async_get_result(goal_handle_); - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - - if ( - callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( - node_->get_logger(), "Failed to cancel action server for [%s]", prev_action_name_.c_str()); - } - - if ( - callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( - node_->get_logger(), "Failed to get result call failed :( for [%s]", - prev_action_name_.c_str()); - } -} - -} // namespace BT diff --git a/panther_manager/include/behaviortree_ros2/bt_service_node.hpp b/panther_manager/include/behaviortree_ros2/bt_service_node.hpp deleted file mode 100644 index bf0ea41b6..000000000 --- a/panther_manager/include/behaviortree_ros2/bt_service_node.hpp +++ /dev/null @@ -1,304 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// Copyright (c) 2023 Davide Faconti -// -// 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. - -#pragma once - -#include -#include -#include -#include -#include "behaviortree_cpp/bt_factory.h" - -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ - -enum ServiceNodeErrorCode { - SERVICE_UNREACHABLE, - SERVICE_TIMEOUT, - INVALID_REQUEST, - SERVICE_ABORTED -}; - -inline const char * toStr(const ServiceNodeErrorCode & err) -{ - switch (err) { - case SERVICE_UNREACHABLE: - return "SERVICE_UNREACHABLE"; - case SERVICE_TIMEOUT: - return "SERVICE_TIMEOUT"; - case INVALID_REQUEST: - return "INVALID_REQUEST"; - case SERVICE_ABORTED: - return "SERVICE_ABORTED"; - } - return nullptr; -} - -/** - * @brief Abstract class use to wrap rclcpp::Client<> - * - * For instance, given the type AddTwoInts described in this tutorial: - * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html - * - * the corresponding wrapper would be: - * - * class AddTwoNumbers: public RosServiceNode - * - * RosServiceNode will try to be non-blocking for the entire duration of the call. - * The derived class must reimplement the virtual methods as described below. - * - * The name of the service will be determined as follows: - * - * 1. If a value is passes in the InputPort "service_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosServiceNode : public BT::ActionNodeBase -{ -public: - // Type definitions - using ServiceClient = typename rclcpp::Client; - using Request = typename ServiceT::Request; - using Response = typename ServiceT::Response; - - /** To register this class into the factory, use: - * - * factory.registerNodeType<>(node_name, params); - */ - explicit RosServiceNode( - const std::string & instance_name, const BT::NodeConfig & conf, - const BT::RosNodeParams & params); - - virtual ~RosServiceNode() = default; - - /** - * @brief Any subclass of RosServiceNode that has ports must implement a - * providedPorts method and call providedBasicPorts in it. - * - * @param addition Additional ports to add to BT port list - * @return PortsList containing basic ports along with node-specific ports - */ - static PortsList providedBasicPorts(PortsList addition) - { - PortsList basic = { - InputPort("service_name", "__default__placeholder__", "Service name")}; - basic.insert(addition.begin(), addition.end()); - return basic; - } - - /** - * @brief Creates list of BT ports - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedPorts() { return providedBasicPorts({}); } - - NodeStatus tick() override final; - - /// The default halt() implementation. - void halt() override; - - /** setRequest is a callback that allows the user to set - * the request message (ServiceT::Request). - * - * @param request the request to be sent to the service provider. - * - * @return false if the request should not be sent. In that case, - * RosServiceNode::onFailure(INVALID_REQUEST) will be called. - */ - virtual bool setRequest(typename Request::SharedPtr & request) = 0; - - /** Callback invoked when the response is received by the server. - * It is up to the user to define if this returns SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) = 0; - - /** Callback invoked when something goes wrong; you can override it. - * It must return either SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) { return NodeStatus::FAILURE; } - -protected: - std::shared_ptr node_; - std::string prev_service_name_; - bool service_name_may_change_ = false; - const std::chrono::milliseconds service_timeout_; - const std::chrono::milliseconds wait_for_service_timeout_; - -private: - typename std::shared_ptr service_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - - std::shared_future future_response_; - - rclcpp::Time time_request_sent_; - NodeStatus on_feedback_state_change_; - bool response_received_; - typename Response::SharedPtr response_; - - bool createClient(const std::string & service_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template -inline RosServiceNode::RosServiceNode( - const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) -: BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - service_timeout_(params.server_timeout), - wait_for_service_timeout_(params.wait_for_server_timeout) -{ - // check port remapping - auto portIt = config().input_ports.find("service_name"); - if (portIt != config().input_ports.end()) { - const std::string & bb_service_name = portIt->second; - - if (bb_service_name.empty() || bb_service_name == "__default__placeholder__") { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } else { - createClient(params.default_port_value); - } - } else if (!isBlackboardPointer(bb_service_name)) { - // If the content of the port "service_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_service_name); - } else { - service_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } - } else { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } else { - createClient(params.default_port_value); - } - } -} - -template -inline bool RosServiceNode::createClient(const std::string & service_name) -{ - if (service_name.empty()) { - throw RuntimeError("service_name is empty"); - } - - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client( - service_name, rmw_qos_profile_services_default, callback_group_); - prev_service_name_ = service_name; - - bool found = service_client_->wait_for_service(wait_for_service_timeout_); - if (!found) { - RCLCPP_ERROR( - node_->get_logger(), "%s: Service with name '%s' is not reachable.", name().c_str(), - prev_service_name_.c_str()); - } - return found; -} - -template -inline NodeStatus RosServiceNode::tick() -{ - // First, check if the service_client_ is valid and that the name of the - // service_name in the port didn't change. - // otherwise, create a new client - if (!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) { - std::string service_name; - getInput("service_name", service_name); - if (prev_service_name_ != service_name) { - createClient(service_name); - } - } - - auto CheckStatus = [](NodeStatus status) { - if (!isStatusCompleted(status)) { - throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or FAILURE"); - } - return status; - }; - - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) { - setStatus(NodeStatus::RUNNING); - - response_received_ = false; - future_response_ = {}; - on_feedback_state_change_ = NodeStatus::RUNNING; - response_ = {}; - - typename Request::SharedPtr request = std::make_shared(); - - if (!setRequest(request)) { - return CheckStatus(onFailure(INVALID_REQUEST)); - } - - future_response_ = service_client_->async_send_request(request).share(); - time_request_sent_ = node_->now(); - - return NodeStatus::RUNNING; - } - - if (status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); - - // FIRST case: check if the goal request has a timeout - if (!response_received_) { - auto const nodelay = std::chrono::milliseconds(0); - auto const timeout = rclcpp::Duration::from_seconds(double(service_timeout_.count()) / 1000); - - auto ret = callback_group_executor_.spin_until_future_complete(future_response_, nodelay); - - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - if ((node_->now() - time_request_sent_) > timeout) { - return CheckStatus(onFailure(SERVICE_TIMEOUT)); - } else { - return NodeStatus::RUNNING; - } - } else { - response_received_ = true; - response_ = future_response_.get(); - future_response_ = {}; - - if (!response_) { - throw std::runtime_error("Request was rejected by the service"); - } - } - } - - // SECOND case: response received - return CheckStatus(onResponseReceived(response_)); - } - return NodeStatus::RUNNING; -} - -template -inline void RosServiceNode::halt() -{ - if (status() == NodeStatus::RUNNING) { - resetStatus(); - } -} - -} // namespace BT diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp deleted file mode 100644 index 41d01060a..000000000 --- a/panther_manager/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ /dev/null @@ -1,168 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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. - -#pragma once - -#include -#include -#include -#include -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ - -/** - * @brief Abstract class to wrap a ROS publisher - * - */ -template -class RosTopicPubNode : public BT::ConditionNode -{ -public: - // Type definitions - using Publisher = typename rclcpp::Publisher; - - /** You are not supposed to instantiate this class directly, the factory will do it. - * To register this class into the factory, use: - * - * RegisterRosAction(factory, params) - * - * Note that if the external_action_client is not set, the constructor will build its own. - * */ - explicit RosTopicPubNode( - const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); - - virtual ~RosTopicPubNode() = default; - - /** - * @brief Any subclass of RosTopicPubNode that has additinal ports must provide a - * providedPorts method and call providedBasicPorts in it. - * - * @param addition Additional ports to add to BT port list - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedBasicPorts(PortsList addition) - { - PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name")}; - basic.insert(addition.begin(), addition.end()); - return basic; - } - - /** - * @brief Creates list of BT ports - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedPorts() { return providedBasicPorts({}); } - - NodeStatus tick() override final; - - /** - * @brief setMessage is a callback invoked in tick to allow the user to pass - * the message to be published. - * - * @param msg the message. - * @return return false if anything is wrong and we must not send the message. - * the Condition will return FAILURE. - */ - virtual bool setMessage(TopicT & msg) = 0; - -protected: - std::shared_ptr node_; - std::string prev_topic_name_; - bool topic_name_may_change_ = false; - -private: - std::shared_ptr publisher_; - - bool createPublisher(const std::string & topic_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template -inline RosTopicPubNode::RosTopicPubNode( - const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) -: BT::ConditionNode(instance_name, conf), node_(params.nh) -{ - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if (portIt != config().input_ports.end()) { - const std::string & bb_topic_name = portIt->second; - - if (bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } else { - createPublisher(params.default_port_value); - } - } else if (!isBlackboardPointer(bb_topic_name)) { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createPublisher(bb_topic_name); - } else { - topic_name_may_change_ = true; - // createPublisher will be invoked in the first tick(). - } - } else { - if (params.default_port_value.empty()) { - throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } else { - createPublisher(params.default_port_value); - } - } -} - -template -inline bool RosTopicPubNode::createPublisher(const std::string & topic_name) -{ - if (topic_name.empty()) { - throw RuntimeError("topic_name is empty"); - } - - publisher_ = node_->create_publisher(topic_name, 1); - prev_topic_name_ = topic_name; - return true; -} - -template -inline NodeStatus RosTopicPubNode::tick() -{ - // First, check if the subscriber_ is valid and that the name of the - // topic_name in the port didn't change. - // otherwise, create a new subscriber - if (!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) { - std::string topic_name; - getInput("topic_name", topic_name); - if (prev_topic_name_ != topic_name) { - createPublisher(topic_name); - } - } - - T msg; - if (!setMessage(msg)) { - return NodeStatus::FAILURE; - } - publisher_->publish(msg); - return NodeStatus::SUCCESS; -} - -} // namespace BT diff --git a/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp b/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp deleted file mode 100644 index 6d3570586..000000000 --- a/panther_manager/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ /dev/null @@ -1,264 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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. - -#pragma once - -#include -#include -#include -#include -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/condition_node.h" - -#include -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ - -/** - * @brief Abstract class to wrap a Topic subscriber. - * Considering the example in the tutorial: - * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html - * - * The corresponding wrapper would be: - * - * class SubscriberNode: RosTopicSubNode - * - * The name of the topic will be determined as follows: - * - * 1. If a value is passes in the InputPort "topic_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosTopicSubNode : public BT::ConditionNode -{ -public: - // Type definitions - using Subscriber = typename rclcpp::Subscription; - -protected: - struct SubscriberInstance - { - void init(std::shared_ptr node, const std::string & topic_name) - { - // create a callback group for this particular instance - callback_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group(callback_group, node->get_node_base_interface()); - - rclcpp::SubscriptionOptions option; - option.callback_group = callback_group; - - // The callback will broadcast to all the instances of RosTopicSubNode - auto callback = [this](const std::shared_ptr msg) { broadcaster(msg); }; - subscriber = node->create_subscription(topic_name, 1, callback, option); - } - - std::shared_ptr subscriber; - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::executors::SingleThreadedExecutor callback_group_executor; - boost::signals2::signal)> broadcaster; - }; - - static std::mutex & registryMutex() - { - static std::mutex sub_mutex; - return sub_mutex; - } - - // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map> & getRegistry() - { - static std::unordered_map> - subscribers_registry; - return subscribers_registry; - } - - std::shared_ptr node_; - std::shared_ptr sub_instance_ = nullptr; - std::shared_ptr last_msg_; - std::string topic_name_; - boost::signals2::connection signal_connection_; - std::string subscriber_key_; - - rclcpp::Logger logger() { return node_->get_logger(); } - -public: - /** You are not supposed to instantiate this class directly, the factory will do it. - * To register this class into the factory, use: - * - * RegisterRosAction(factory, params) - * - * Note that if the external_action_client is not set, the constructor will build its own. - * */ - explicit RosTopicSubNode( - const std::string & instance_name, const BT::NodeConfig & conf, const RosNodeParams & params); - - virtual ~RosTopicSubNode() - { - signal_connection_.disconnect(); - // remove the subscribers from the static registry when the ALL the - // instances of RosTopicSubNode are destroyed (i.e., the tree is destroyed) - if (sub_instance_) { - sub_instance_.reset(); - std::unique_lock lk(registryMutex()); - auto & registry = getRegistry(); - auto it = registry.find(subscriber_key_); - // when the reference count is 1, means that the only instance is owned by the - // registry itself. Delete it - if (it != registry.end() && it->second.use_count() <= 1) { - registry.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str()); - } - } - } - - /** - * @brief Any subclass of RosTopicNode that accepts parameters must provide a - * providedPorts method and call providedBasicPorts in it. - * @param addition Additional ports to add to BT port list - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedBasicPorts(PortsList addition) - { - PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name")}; - basic.insert(addition.begin(), addition.end()); - return basic; - } - - /** - * @brief Creates list of BT ports - * @return PortsList Containing basic ports along with node-specific ports - */ - static PortsList providedPorts() { return providedBasicPorts({}); } - - NodeStatus tick() override final; - - /** Callback invoked in the tick. You must return either SUCCESS of FAILURE - * - * @param last_msg the latest message received, since the last tick. - * Will be empty if no new message received. - * - * @return the new status of the Node, based on last_msg - */ - virtual NodeStatus onTick(const std::shared_ptr & last_msg) = 0; - -private: - bool createSubscriber(const std::string & topic_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template -inline RosTopicSubNode::RosTopicSubNode( - const std::string & instance_name, const NodeConfig & conf, const RosNodeParams & params) -: BT::ConditionNode(instance_name, conf), node_(params.nh) -{ - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if (portIt != config().input_ports.end()) { - const std::string & bb_topic_name = portIt->second; - - if (bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { - if (params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } else { - createSubscriber(params.default_port_value); - } - } else if (!isBlackboardPointer(bb_topic_name)) { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createSubscriber(bb_topic_name); - } else { - // do nothing - // createSubscriber will be invoked in the first tick(). - } - } else { - if (params.default_port_value.empty()) { - throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } else { - createSubscriber(params.default_port_value); - } - } -} - -template -inline bool RosTopicSubNode::createSubscriber(const std::string & topic_name) -{ - if (topic_name.empty()) { - throw RuntimeError("topic_name is empty"); - } - if (sub_instance_) { - throw RuntimeError("Can't call createSubscriber more than once"); - } - - // find SubscriberInstance in the registry - std::unique_lock lk(registryMutex()); - subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; - - auto & registry = getRegistry(); - auto it = registry.find(subscriber_key_); - if (it == registry.end()) { - it = registry.insert({subscriber_key_, std::make_shared()}).first; - sub_instance_ = it->second; - sub_instance_->init(node_, topic_name); - - RCLCPP_INFO( - logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), topic_name.c_str()); - } else { - sub_instance_ = it->second; - } - - // add "this" as received of the broadcaster - signal_connection_ = - sub_instance_->broadcaster.connect([this](const std::shared_ptr msg) { last_msg_ = msg; }); - - topic_name_ = topic_name; - return true; -} - -template -inline NodeStatus RosTopicSubNode::tick() -{ - // First, check if the subscriber_ is valid and that the name of the - // topic_name in the port didn't change. - // otherwise, create a new subscriber - if (!sub_instance_) { - std::string topic_name; - getInput("topic_name", topic_name); - createSubscriber(topic_name); - } - - auto CheckStatus = [](NodeStatus status) { - if (!isStatusCompleted(status)) { - throw std::logic_error( - "RosTopicSubNode: the callback must return" - "either SUCCESS or FAILURE"); - } - return status; - }; - sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus(onTick(last_msg_)); - last_msg_ = nullptr; - - return status; -} - -} // namespace BT diff --git a/panther_manager/include/behaviortree_ros2/plugins.hpp b/panther_manager/include/behaviortree_ros2/plugins.hpp deleted file mode 100644 index a7b19510f..000000000 --- a/panther_manager/include/behaviortree_ros2/plugins.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) 2023 Davide Faconti -// -// 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. - -#pragma once - -#include - -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_cpp/utils/shared_library.h" -#include "behaviortree_ros2/ros_node_params.hpp" - -// Use this macro to generate a plugin for: -// -// - BT::RosActionNode -// - BT::RosServiceNode -// - BT::RosTopicPubNode -// - BT::RosTopicSubNode -// -// - First argument: type to register (name of the class) -// - Second argument: string with the registration name -// -// Usage example: -// CreateRosNodePlugin(MyClassName, "MyClassName"); - -#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ - BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin( \ - BT::BehaviorTreeFactory & factory, const BT::RosNodeParams & params) \ - { \ - factory.registerNodeType(REGISTRATION_NAME, params); \ - } - -/** - * @brief RegisterRosNode function used to load a plugin and register - * the containing Node definition. - * - * @param factory the factory where the node should be registered. - * @param filepath path to the plugin. - * @param params parameters to pass to the instances of the Node. - */ -inline void RegisterRosNode( - BT::BehaviorTreeFactory & factory, const std::filesystem::path & filepath, - const BT::RosNodeParams & params) -{ - BT::SharedLibrary loader(filepath.generic_string()); - typedef void (*Func)(BT::BehaviorTreeFactory &, const BT::RosNodeParams &); - auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); - func(factory, params); -} diff --git a/panther_manager/include/behaviortree_ros2/ros_node_params.hpp b/panther_manager/include/behaviortree_ros2/ros_node_params.hpp deleted file mode 100644 index 0802350b8..000000000 --- a/panther_manager/include/behaviortree_ros2/ros_node_params.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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. - -#pragma once - -#include -#include -#include -#include - -namespace BT -{ - -struct RosNodeParams -{ - std::shared_ptr nh; - - // This has different meaning based on the context: - // - // - RosActionNode: name of the action server - // - RosServiceNode: name of the service - // - RosTopicPubNode: name of the topic to publish to - // - RosTopicSubNode: name of the topic to subscribe to - std::string default_port_value; - - // parameters used only by service client and action clients - - // timeout when sending a request - std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); - // timeout used when detecting the server the first time - std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); -}; - -} // namespace BT diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index 3b0923a48..8e1e652e1 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -15,8 +15,11 @@ #ifndef PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#include + #include #include + #include namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 4ef263e23..2428a4703 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -15,10 +15,13 @@ #ifndef PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#include + #include -#include #include +#include + namespace panther_manager { diff --git a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp index 4bc6607b0..59add7187 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp @@ -15,8 +15,11 @@ #ifndef PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ #define PANTHER_MANAGER_CALL_TRIGGER_SERVICE_NODE_HPP_ +#include + #include #include + #include namespace panther_manager @@ -33,7 +36,7 @@ class CallTriggerService : public BT::RosServiceNode static BT::PortsList providedPorts() { return providedBasicPorts({}); } - virtual bool setRequest(typename Request::SharedPtr & /* request*/) override; + virtual bool setRequest(typename Request::SharedPtr & /*request*/) override; virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override; }; diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index 91caa7a36..c3235bbcc 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -47,7 +47,7 @@ class ShutdownHost user_(""), port_(22), command_(""), - timeout_(5000), + timeout_ms_(5000), ping_for_success_(true), hash_(std::hash{}("")) { @@ -60,7 +60,7 @@ class ShutdownHost user_(user), port_(port), command_(command), - timeout_(static_cast(timeout * 1000)), + timeout_ms_(static_cast(timeout * 1000)), ping_for_success_(ping_for_success), hash_(std::hash{}(ip + user + std::to_string(port))), state_(ShutdownHostState::IDLE) @@ -163,7 +163,7 @@ class ShutdownHost const std::string user_; const int port_; const std::string command_; - const std::chrono::milliseconds timeout_; + const std::chrono::milliseconds timeout_ms_; const bool ping_for_success_; const std::size_t hash_; ShutdownHostState state_; @@ -213,7 +213,7 @@ class ShutdownHost auto now = std::chrono::steady_clock::now(); auto elapsed = std::chrono::duration_cast(now - command_time_); - return elapsed > timeout_ && is_available(); + return elapsed > timeout_ms_ && is_available(); } void ssh_execute_command(const std::string & command) diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 35646b605..5f67d12de 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -41,9 +41,7 @@ class ShutdownHosts : public BT::StatefulActionNode explicit ShutdownHosts(const std::string & name, const BT::NodeConfig & conf) : BT::StatefulActionNode(name, conf) { - // TODO: @delihus What is the name of ros::this_node? - node_name_ = name; - logger_ = std::make_shared(rclcpp::get_logger(node_name_)); + this->logger_ = std::make_shared(rclcpp::get_logger(name)); } virtual ~ShutdownHosts() = default; @@ -55,18 +53,16 @@ class ShutdownHosts : public BT::StatefulActionNode virtual BT::NodeStatus post_process() { // return success only when all hosts succeeded - if (failed_hosts_.size() == 0) { + if (this->failed_hosts_.size() == 0) { return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; } - std::string get_node_name() const { return node_name_; } - std::vector const get_failed_hosts() { return failed_hosts_; } + std::vector const get_failed_hosts() { return this->failed_hosts_; } private: std::size_t check_host_index_ = 0; - std::string node_name_; std::vector> hosts_; std::vector hosts_to_check_; std::vector skipped_hosts_; @@ -76,65 +72,65 @@ class ShutdownHosts : public BT::StatefulActionNode BT::NodeStatus onStart() { - update_hosts(hosts_); - remove_duplicate_hosts(hosts_); - if (hosts_.size() <= 0) { - RCLCPP_ERROR_STREAM(*logger_, "Hosts list is empty! Check configuration!"); + update_hosts(this->hosts_); + remove_duplicate_hosts(this->hosts_); + if (this->hosts_.size() <= 0) { + RCLCPP_ERROR_STREAM(*this->logger_, "Hosts list is empty! Check configuration!"); return BT::NodeStatus::FAILURE; } - hosts_to_check_.resize(hosts_.size()); - std::iota(hosts_to_check_.begin(), hosts_to_check_.end(), 0); + this->hosts_to_check_.resize(this->hosts_.size()); + std::iota(this->hosts_to_check_.begin(), this->hosts_to_check_.end(), 0); return BT::NodeStatus::RUNNING; } BT::NodeStatus onRunning() { - if (hosts_to_check_.size() <= 0) { + if (this->hosts_to_check_.size() <= 0) { return post_process(); } - if (check_host_index_ >= hosts_to_check_.size()) { - check_host_index_ = 0; + if (this->check_host_index_ >= this->hosts_to_check_.size()) { + this->check_host_index_ = 0; } - auto host_index = hosts_to_check_[check_host_index_]; - auto host = hosts_[host_index]; + auto host_index = this->hosts_to_check_[this->check_host_index_]; + auto host = this->hosts_[host_index]; host->call(); switch (host->get_state()) { case ShutdownHostState::RESPONSE_RECEIVED: RCLCPP_INFO_STREAM( - *logger_, "Device at: " << host->get_ip() << " response:\n" - << host->get_response()); + *this->logger_, "Device at: " << host->get_ip() << " response:\n" + << host->get_response()); check_host_index_++; break; case ShutdownHostState::SUCCESS: - RCLCPP_INFO_STREAM(*logger_, "Successfully shutdown device at: " << host->get_ip()); - succeeded_hosts_.push_back(host_index); - hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + RCLCPP_INFO_STREAM(*this->logger_, "Successfully shutdown device at: " << host->get_ip()); + this->succeeded_hosts_.push_back(host_index); + this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); break; case ShutdownHostState::FAILURE: RCLCPP_WARN_STREAM( - *logger_, + *this->logger_, "Failed to shutdown device at: " << host->get_ip() << " Error: " << host->get_error()); - failed_hosts_.push_back(host_index); - hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + this->failed_hosts_.push_back(host_index); + this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); break; case ShutdownHostState::SKIPPED: RCLCPP_WARN_STREAM( - *logger_, "Device at: " << host->get_ip() << " not available, skipping..."); + *this->logger_, "Device at: " << host->get_ip() << " not available, skipping..."); - skipped_hosts_.push_back(host_index); - hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + this->skipped_hosts_.push_back(host_index); + this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); break; default: - check_host_index_++; + this->check_host_index_++; break; } @@ -154,10 +150,10 @@ class ShutdownHosts : public BT::StatefulActionNode return false; } else { RCLCPP_WARN_STREAM( - *logger_, "Found duplicate host: " << host->get_ip() - << " Processing only the " - "first " - "occurrence."); + *this->logger_, "Found duplicate host: " << host->get_ip() + << " Processing only the " + "first " + "occurrence."); return true; } }), @@ -166,7 +162,7 @@ class ShutdownHosts : public BT::StatefulActionNode void onHalted() { - for (auto & host : hosts_) { + for (auto & host : this->hosts_) { host->close_connection(); } } diff --git a/panther_manager/package.xml b/panther_manager/package.xml index ebb245c49..6f5a0fd2f 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -4,7 +4,7 @@ panther_manager 0.0.1 - The panther_manager package + Set of nodes used for high level management of Husarion Panther robot Husarion Apache License 2.0 @@ -14,6 +14,8 @@ Jakub Delicat + ament_cmake + behaviortree_cpp iputils-ping libboost-dev diff --git a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp b/panther_manager/plugins/action/call_set_bool_service_node.cpp similarity index 80% rename from panther_manager/src/plugins/action/call_set_bool_service_node.cpp rename to panther_manager/plugins/action/call_set_bool_service_node.cpp index fea389148..634b9f7e0 100644 --- a/panther_manager/src/plugins/action/call_set_bool_service_node.cpp +++ b/panther_manager/plugins/action/call_set_bool_service_node.cpp @@ -29,13 +29,14 @@ BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::S { if (!response->success) { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + this->node_->get_logger(), + "Failed to call " << this->prev_service_name_ << " service, message: " << response->message); return BT::NodeStatus::FAILURE; } RCLCPP_DEBUG_STREAM( - node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + this->node_->get_logger(), "Successfully called " + << this->prev_service_name_ + << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp b/panther_manager/plugins/action/call_set_led_animation_service_node.cpp similarity index 71% rename from panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp rename to panther_manager/plugins/action/call_set_led_animation_service_node.cpp index 3b27c98a2..5ade21a00 100644 --- a/panther_manager/src/plugins/action/call_set_led_animation_service_node.cpp +++ b/panther_manager/plugins/action/call_set_led_animation_service_node.cpp @@ -14,6 +14,10 @@ #include +#include + +#include + namespace panther_manager { @@ -21,19 +25,19 @@ bool CallSetLedAnimationService::setRequest(typename Request::SharedPtr & reques { unsigned animation_id; if (!getInput("id", animation_id)) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [id]"); + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "Failed to get input [id]"); return false; } request->animation.id = static_cast(animation_id); if (!getInput("param", request->animation.param)) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [param]"); + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "Failed to get input [param]"); return false; } if (!getInput("repeating", request->repeating)) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [repeating]"); + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "Failed to get input [repeating]"); return false; } @@ -45,13 +49,14 @@ BT::NodeStatus CallSetLedAnimationService::onResponseReceived( { if (!response->success) { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + this->node_->get_logger(), + "Failed to call " << this->prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } RCLCPP_DEBUG_STREAM( - node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + this->node_->get_logger(), "Successfully called " + << this->prev_service_name_ + << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/call_trigger_service_node.cpp b/panther_manager/plugins/action/call_trigger_service_node.cpp similarity index 76% rename from panther_manager/src/plugins/action/call_trigger_service_node.cpp rename to panther_manager/plugins/action/call_trigger_service_node.cpp index 530ceb279..049eb1cc2 100644 --- a/panther_manager/src/plugins/action/call_trigger_service_node.cpp +++ b/panther_manager/plugins/action/call_trigger_service_node.cpp @@ -14,6 +14,8 @@ #include +#include + namespace panther_manager { @@ -23,13 +25,14 @@ BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::S { if (!response->success) { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Failed to call " << prev_service_name_ << "service, message: " << response->message); + this->node_->get_logger(), + "Failed to call " << this->prev_service_name_ << "service, message: " << response->message); return BT::NodeStatus::FAILURE; } RCLCPP_DEBUG_STREAM( - node_->get_logger(), - "Successfully called " << prev_service_name_ << " service, message: " << response->message); + this->node_->get_logger(), "Successfully called " + << this->prev_service_name_ + << " service, message: " << response->message); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp similarity index 89% rename from panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp rename to panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp index ea0b571c5..8ac9a37bb 100644 --- a/panther_manager/src/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -34,19 +34,19 @@ void ShutdownHostsFromFile::update_hosts(std::vector("shutdown_hosts_file", shutdown_hosts_file) || shutdown_hosts_file == "") { - throw(BT::RuntimeError("[", name(), "] Failed to get input [shutdown_hosts_file]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [shutdown_hosts_file]")); } YAML::Node shutdown_hosts; try { shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); } catch (const YAML::Exception & e) { - throw BT::RuntimeError("[" + name() + "] Error loading YAML file: " + e.what()); + throw BT::RuntimeError("[" + this->name() + "] Error loading YAML file: " + e.what()); } for (const auto & host : shutdown_hosts["hosts"]) { if (!host["ip"] || !host["username"]) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(get_node_name()), "Missing info for remote host!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(this->name()), "Missing info for remote host!"); continue; } diff --git a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp b/panther_manager/plugins/action/shutdown_single_host_node.cpp similarity index 78% rename from panther_manager/src/plugins/action/shutdown_single_host_node.cpp rename to panther_manager/plugins/action/shutdown_single_host_node.cpp index 5aa6f084d..9ec250f09 100644 --- a/panther_manager/src/plugins/action/shutdown_single_host_node.cpp +++ b/panther_manager/plugins/action/shutdown_single_host_node.cpp @@ -30,32 +30,32 @@ void ShutdownSingleHost::update_hosts(std::vector> { std::string ip; if (!getInput("ip", ip) || ip == "") { - throw(BT::RuntimeError("[", name(), "] Failed to get input [ip]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [ip]")); } std::string user; if (!getInput("user", user) || user == "") { - throw(BT::RuntimeError("[", name(), "] Failed to get input [user]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [user]")); } unsigned port; if (!getInput("port", port)) { - throw(BT::RuntimeError("[", name(), "] Failed to get input [port]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [port]")); } std::string command; if (!getInput("command", command) || command == "") { - throw(BT::RuntimeError("[", name(), "] Failed to get input [command]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [command]")); } float timeout; if (!getInput("timeout", timeout)) { - throw(BT::RuntimeError("[", name(), "] Failed to get input [timeout]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [timeout]")); } bool ping_for_success; if (!getInput("ping_for_success", ping_for_success)) { - throw(BT::RuntimeError("[", name(), "] Failed to get input [ping_for_success]")); + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [ping_for_success]")); } hosts.push_back( diff --git a/panther_manager/src/plugins/action/signal_shutdown_node.cpp b/panther_manager/plugins/action/signal_shutdown_node.cpp similarity index 85% rename from panther_manager/src/plugins/action/signal_shutdown_node.cpp rename to panther_manager/plugins/action/signal_shutdown_node.cpp index 802624c55..0f1fa190f 100644 --- a/panther_manager/src/plugins/action/signal_shutdown_node.cpp +++ b/panther_manager/plugins/action/signal_shutdown_node.cpp @@ -18,19 +18,18 @@ #include #include -#include namespace panther_manager { BT::NodeStatus SignalShutdown::tick() { - auto reason = getInput("reason").value(); + auto reason = this->getInput("reason").value(); std::pair signal_shutdown; signal_shutdown.first = true; signal_shutdown.second = reason; - config().blackboard->set>("signal_shutdown", signal_shutdown); + this->config().blackboard->set>("signal_shutdown", signal_shutdown); return BT::NodeStatus::SUCCESS; } diff --git a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp b/panther_manager/plugins/decorator/tick_after_timeout_node.cpp similarity index 82% rename from panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp rename to panther_manager/plugins/decorator/tick_after_timeout_node.cpp index 19ca4a004..d00eef105 100644 --- a/panther_manager/src/plugins/decorator/tick_after_timeout_node.cpp +++ b/panther_manager/plugins/decorator/tick_after_timeout_node.cpp @@ -20,14 +20,14 @@ namespace panther_manager TickAfterTimeout::TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf) : BT::DecoratorNode(name, conf) { - last_success_time_ = std::chrono::steady_clock::now(); + this->last_success_time_ = std::chrono::steady_clock::now(); } BT::NodeStatus TickAfterTimeout::tick() { float timeout; - if (!getInput("timeout", timeout)) { - throw(BT::RuntimeError("[", name(), "] Failed to get input [timeout]")); + if (!this->getInput("timeout", timeout)) { + throw(BT::RuntimeError("[", this->name(), "] Failed to get input [timeout]")); } timeout_ = std::chrono::duration(timeout); @@ -39,15 +39,15 @@ BT::NodeStatus TickAfterTimeout::tick() return BT::NodeStatus::SKIPPED; } - setStatus(BT::NodeStatus::RUNNING); - auto child_status = child()->executeTick(); + this->setStatus(BT::NodeStatus::RUNNING); + auto child_status = this->child()->executeTick(); if (child_status == BT::NodeStatus::SUCCESS) { last_success_time_ = std::chrono::steady_clock::now(); } if (child_status != BT::NodeStatus::RUNNING) { - resetChild(); + this->resetChild(); } return child_status; diff --git a/panther_manager/src/bt_ros2.cpp b/panther_manager/src/bt_ros2.cpp deleted file mode 100644 index 4b307759a..000000000 --- a/panther_manager/src/bt_ros2.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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 "behaviortree_ros2/bt_action_node.hpp" -#include "behaviortree_ros2/bt_service_node.hpp" -#include "behaviortree_ros2/bt_topic_pub_node.hpp" -#include "behaviortree_ros2/bt_topic_sub_node.hpp" From 8295f4ce7aeb9a9aad38021b8e6bb61eeb577431 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Mar 2024 17:03:36 +0000 Subject: [PATCH 23/35] Changed utils names and moved start stop to constructor and destructor Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 2 +- ...n_test_utils.hpp => plugin_test_utils.hpp} | 32 +++++--- ...n_test_utils.cpp => plugin_test_utils.cpp} | 31 ++++---- .../test_call_set_bool_service_node.cpp | 57 ++++++-------- ...st_call_set_led_animation_service_node.cpp | 76 ++++++++----------- .../test_call_trigger_service_node.cpp | 39 ++++------ .../test_shutdown_hosts_from_file_node.cpp | 30 +++----- .../test_shutdown_single_host_node.cpp | 35 ++++----- .../plugins/test_signal_shutdown_node.cpp | 36 ++------- .../plugins/test_tick_after_timeout_node.cpp | 21 +++-- 10 files changed, 153 insertions(+), 206 deletions(-) rename panther_manager/test/plugins/include/{panther_manager_plugin_test_utils.hpp => plugin_test_utils.hpp} (90%) rename panther_manager/test/plugins/src/{panther_manager_plugin_test_utils.cpp => plugin_test_utils.cpp} (84%) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index ba591754a..14b29c10d 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -61,7 +61,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_library(${PROJECT_NAME}_test_plugin_utils - test/plugins/src/panther_manager_plugin_test_utils.cpp) + test/plugins/src/plugin_test_utils.cpp) target_include_directories(${PROJECT_NAME}_test_plugin_utils PUBLIC test/plugins/include) diff --git a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp similarity index 90% rename from panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp rename to panther_manager/test/plugins/include/plugin_test_utils.hpp index 598deeb2a..3e9ecbe16 100644 --- a/panther_manager/test/plugins/include/panther_manager_plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -15,14 +15,21 @@ #ifndef PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ #define PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ +#include +#include +#include #include +#include #include -#include +#include #include + #include #include +#include + #include #include #include @@ -31,9 +38,7 @@ #include #include -#include - -namespace panther_manager_plugin_test +namespace panther_manager::plugin_test_utils { struct BehaviorTreePluginDescription @@ -42,21 +47,24 @@ struct BehaviorTreePluginDescription std::map params; }; -class PantherManagerPluginTestUtils +class PluginTestUtils { public: + PluginTestUtils(); + + ~PluginTestUtils(); + std::string BuildBehaviorTree( const std::string & plugin_name, const std::map & service, double tick_after_timeout); - BT::Tree & CreateTree( + void CreateTree( const std::string & plugin_name, const std::map & service, double tick_after_timeout = std::numeric_limits::quiet_NaN()); - BT::BehaviorTreeFactory & GetFactory(); + BT::Tree & GetTree(); - void Start(); - void Stop(); + BT::BehaviorTreeFactory & GetFactory(); void CreateSetBoolServiceServer( std::function< @@ -87,18 +95,18 @@ class PantherManagerPluginTestUtils void spin_executor(); - const std::string header_ = R"( + const std::string tree_header_ = R"( )"; - const std::string footer_ = R"( + const std::string tree_footer_ = R"( )"; }; -} // namespace panther_manager_plugin_test +} // namespace panther_manager::plugin_test_utils #endif // PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ diff --git a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp similarity index 84% rename from panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp rename to panther_manager/test/plugins/src/plugin_test_utils.cpp index 1333ab475..a7711ac7c 100644 --- a/panther_manager/test/plugins/src/panther_manager_plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/plugin_test_utils.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace panther_manager_plugin_test +namespace panther_manager::plugin_test_utils { -std::string PantherManagerPluginTestUtils::BuildBehaviorTree( +std::string PluginTestUtils::BuildBehaviorTree( const std::string & plugin_name, const std::map & service, double tick_after_timeout) { std::stringstream bt; - bt << header_ << std::endl; + bt << tree_header_ << std::endl; if (not std::isnan(tick_after_timeout)) { bt << "\t\t\t" << std::endl; } @@ -40,23 +40,24 @@ std::string PantherManagerPluginTestUtils::BuildBehaviorTree( bt << "\t\t\t" << std::endl; } - bt << footer_; + bt << tree_footer_; return bt.str(); } -BT::Tree & PantherManagerPluginTestUtils::CreateTree( +void PluginTestUtils::CreateTree( const std::string & plugin_name, const std::map & service, double tick_after_timeout) { auto xml_text = BuildBehaviorTree(plugin_name, service, tick_after_timeout); tree_ = factory_.createTreeFromText(xml_text); - return tree_; } -BT::BehaviorTreeFactory & PantherManagerPluginTestUtils::GetFactory() { return factory_; } +BT::Tree & PluginTestUtils::GetTree() { return tree_; } -void PantherManagerPluginTestUtils::Start() +BT::BehaviorTreeFactory & PluginTestUtils::GetFactory() { return factory_; } + +PluginTestUtils::PluginTestUtils() { rclcpp::init(0, nullptr); bt_node_ = std::make_shared("test_panther_manager_node"); @@ -73,7 +74,7 @@ void PantherManagerPluginTestUtils::Start() factory_.registerNodeType("TickAfterTimeout"); } -void PantherManagerPluginTestUtils::Stop() +PluginTestUtils::~PluginTestUtils() { bt_node_.reset(); rclcpp::shutdown(); @@ -83,7 +84,7 @@ void PantherManagerPluginTestUtils::Stop() } } -void PantherManagerPluginTestUtils::CreateSetBoolServiceServer( +void PluginTestUtils::CreateSetBoolServiceServer( std::function< void(std_srvs::srv::SetBool::Request::SharedPtr, std_srvs::srv::SetBool::Response::SharedPtr)> service_callback) @@ -96,7 +97,7 @@ void PantherManagerPluginTestUtils::CreateSetBoolServiceServer( executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::CreateTriggerServiceServer( +void PluginTestUtils::CreateTriggerServiceServer( std::function< void(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr)> service_callback) @@ -109,7 +110,7 @@ void PantherManagerPluginTestUtils::CreateTriggerServiceServer( executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( +void PluginTestUtils::CreateSetLEDAnimationServiceServer( std::function @@ -124,6 +125,6 @@ void PantherManagerPluginTestUtils::CreateSetLEDAnimationServiceServer( executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } -void PantherManagerPluginTestUtils::spin_executor() { executor_->spin(); } +void PluginTestUtils::spin_executor() { executor_->spin(); } -} // namespace panther_manager_plugin_test +} // namespace panther_manager::plugin_test_utils diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index 5f2bfe774..80c906f8e 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -20,8 +20,7 @@ #include #include - -#include +#include void ServiceFailedCallback( const std_srvs::srv::SetBool::Request::SharedPtr request, @@ -61,83 +60,76 @@ TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", service); }); - test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetBoolService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetBoolService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetBoolService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetBoolService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestCallSetBoolService, wrong_service_value_defined) @@ -145,14 +137,13 @@ TEST(TestCallSetBoolService, wrong_service_value_defined) std::map service = { {"service_name", "set_bool"}, {"data", "wrong_bool"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetBoolService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 42a8a9f4a..816b41cb7 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -20,8 +20,7 @@ #include #include - -#include +#include void ServiceFailedCallback( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, @@ -86,10 +85,9 @@ TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", service); }); - test_utils.Stop(); } TEST( @@ -98,11 +96,10 @@ TEST( std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW( { test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST( @@ -112,14 +109,13 @@ TEST( std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST( @@ -129,16 +125,15 @@ TEST( std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST( @@ -148,16 +143,15 @@ TEST( std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) @@ -165,16 +159,15 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success std::map service = { {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) @@ -182,16 +175,15 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) @@ -199,14 +191,13 @@ TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "wrong_bool"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) @@ -214,14 +205,13 @@ TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) std::map service = { {"service_name", "set_led_animation"}, {"id", "-5"}, {"param", ""}, {"repeating", "true"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallSetLedAnimationService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallSetLedAnimationService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index 11f5e9f43..553d774fc 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -21,8 +21,7 @@ #include #include - -#include +#include void ServiceFailedCallback( const std_srvs::srv::Trigger::Request::SharedPtr request, @@ -48,67 +47,61 @@ TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) { std::map service = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); - test_utils.Stop(); } TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) { std::map service = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) { std::map service = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallTriggerService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestCallTriggerService, good_trigger_call_service_success) { std::map service = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallTriggerService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - test_utils.Stop(); } TEST(TestCallTriggerService, wrong_trigger_call_service_failure) { std::map service = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallTriggerService", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallTriggerService", service); + auto & tree = test_utils.GetTree(); test_utils.CreateTriggerServiceServer(ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index 275559879..507bfc7fa 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -22,28 +22,24 @@ #include #include - -#include +#include TEST(TestShutdownHostsFromFile, good_loading_shutdown_hosts_from_file_plugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownHostsFromFile", service); }); - test_utils.Stop(); } TEST(TestShutdownHostsFromFile, wrong_plugin_name_loading_shutdown_hosts_from_file_plugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW({ test_utils.CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) @@ -51,13 +47,12 @@ TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) const std::string file_path = "/tmp/test_wrong_cannot_find_file_shutdown_hosts_from_file"; const std::map service = {{"shutdown_hosts_file", file_path}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("ShutdownHostsFromFile", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; - EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); + test_utils.CreateTree("ShutdownHostsFromFile", service); + auto & tree = test_utils.GetTree(); - test_utils.Stop(); + EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); } TEST(TestShutdownHostsFromFile, good_shutdown_hosts_from_file) @@ -87,17 +82,16 @@ TEST(TestShutdownHostsFromFile, good_shutdown_hosts_from_file) config_file.close(); const std::map service = {{"shutdown_hosts_file", config_file_path}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("ShutdownHostsFromFile", service); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + auto & tree = test_utils.GetTree(); + test_utils.CreateTree("ShutdownHostsFromFile", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); std::filesystem::remove(test_file_path); std::filesystem::remove(config_file_path); - - test_utils.Stop(); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 918712f57..1a88ae722 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) { @@ -30,11 +30,9 @@ TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownSingleHost", service); }); - test_utils.Stop(); } TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plugin) @@ -44,10 +42,9 @@ TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plug {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW({ test_utils.CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestShutdownSingleHost, good_touch_command) @@ -64,17 +61,16 @@ TEST(TestShutdownSingleHost, good_touch_command) {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); EXPECT_TRUE(std::filesystem::exists(file_path)); std::filesystem::remove(file_path); - test_utils.Stop(); } TEST(TestShutdownSingleHost, wrong_command) @@ -87,15 +83,13 @@ TEST(TestShutdownSingleHost, wrong_command) {"timeout", "0.2"}, {"user", "husarion"}, }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } TEST(TestShutdownSingleHost, wrong_user) @@ -108,15 +102,14 @@ TEST(TestShutdownSingleHost, wrong_user) {"timeout", "5.0"}, {"user", "user_what_does_not_exists"}, }; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("ShutdownSingleHost", service); + auto & tree = test_utils.GetTree(); test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.CreateTree("ShutdownSingleHost", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); - - test_utils.Stop(); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp index 3a71478c8..d6b051d7f 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -21,52 +21,32 @@ #include #include -#include +#include TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) { std::map service = {{"reason", "Test shutdown."}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); - test_utils.Stop(); } TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) { std::map service = {}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW({ test_utils.CreateTree("WrongSignalShutdown", service); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestSignalShutdown, good_check_reason_blackboard_value) { std::map service = {{"reason", "Test shutdown."}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("SignalShutdown", service); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - - auto blackboard = tree.rootBlackboard(); - auto got_value = blackboard->get>("signal_shutdown"); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; - EXPECT_EQ(got_value.first, true); - EXPECT_EQ(got_value.second, service["reason"]); - test_utils.Stop(); -} - -TEST(TestSignalShutdown, wrong_check_reason_blackboard_value) -{ - std::map service = {{"reason", "Test shutdown."}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("SignalShutdown", service); + test_utils.CreateTree("SignalShutdown", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -75,7 +55,7 @@ TEST(TestSignalShutdown, wrong_check_reason_blackboard_value) auto got_value = blackboard->get>("signal_shutdown"); EXPECT_EQ(got_value.first, true); - EXPECT_FALSE(got_value.second == "Wrong reason!"); + EXPECT_EQ(got_value.second, service["reason"]); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index fa1a5f422..883c613e5 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include inline static std::size_t counter = 0; @@ -43,31 +43,30 @@ TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) { std::map trigger_node = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); }); - test_utils.Stop(); } TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) { std::map trigger_node = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + EXPECT_THROW( { test_utils.CreateTree("WrongTriggerService", trigger_node, 0.1); }, BT::RuntimeError); - test_utils.Stop(); } TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) { std::map trigger_node = {{"service_name", "trigger"}}; - panther_manager_plugin_test::PantherManagerPluginTestUtils test_utils; - test_utils.Start(); - auto & tree = test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); + panther_manager::plugin_test_utils::PluginTestUtils test_utils; + + test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); + auto & tree = test_utils.GetTree(); test_utils.CreateTriggerServiceServer(CounterIncrease); EXPECT_EQ(counter, 0); @@ -90,8 +89,6 @@ TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) EXPECT_EQ(duration, 100); } EXPECT_EQ(counter, 4); - - test_utils.Stop(); } int main(int argc, char ** argv) From ea26a37295a5b6769b4e45e24f67d1dc23689370 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Mar 2024 17:22:15 +0000 Subject: [PATCH 24/35] applied panther_utils Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 3 +- .../action/shutdown_hosts_from_file_node.hpp | 1 + panther_manager/package.xml | 1 + .../action/shutdown_hosts_from_file_node.cpp | 42 ++++++++----------- 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 14b29c10d..18405ad8c 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -15,7 +15,8 @@ set(PACKAGE_INCLUDE_DEPENDS panther_msgs libssh yaml-cpp - behaviortree_ros2) + behaviortree_ros2 + panther_utils) foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 9923b1740..a1132bf1f 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace panther_manager { diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 6f5a0fd2f..8d6d0cff1 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -20,6 +20,7 @@ iputils-ping libboost-dev libssh-dev + panther_utils rclcpp rclcpp_action std_srvs diff --git a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp index 8ac9a37bb..f9c5a4c56 100644 --- a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -44,33 +44,25 @@ void ShutdownHostsFromFile::update_hosts(std::vectorname() + "] Error loading YAML file: " + e.what()); } - for (const auto & host : shutdown_hosts["hosts"]) { - if (!host["ip"] || !host["username"]) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(this->name()), "Missing info for remote host!"); - continue; - } + try { + for (const auto & host : shutdown_hosts["hosts"]) { + if (!host["ip"] || !host["username"]) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(this->name()), "Missing info for remote host!"); + continue; + } - auto ip = host["ip"].as(); - auto user = host["username"].as(); - unsigned port = 22; - if (host["port"]) { - port = host["port"].as(); - } - std::string command = "sudo shutdown now"; - if (host["command"]) { - command = host["command"].as(); + auto ip = panther_utils::GetYAMLKeyValue(host, "ip"); + auto user = panther_utils::GetYAMLKeyValue(host, "user"); + auto port = panther_utils::GetYAMLKeyValue(host, "port", 22); + auto command = panther_utils::GetYAMLKeyValue( + host, "command", "sudo shutdown now"); + auto timeout = panther_utils::GetYAMLKeyValue(host, "timeout", 5.0); + auto ping_for_success = panther_utils::GetYAMLKeyValue(host, "ping_for_success", true); + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); } - float timeout = 5.0; - if (host["timeout"]) { - timeout = host["timeout"].as(); - } - bool ping_for_success = true; - if (host["ping_for_success"]) { - ping_for_success = host["ping_for_success"].as(); - } - - hosts.push_back( - std::make_shared(ip, user, port, command, timeout, ping_for_success)); + } catch (const std::runtime_error & e) { + throw BT::RuntimeError("[" + this->name() + "]: " + e.what()); } } From d975d613760d527c312f2d5aa0be3ccf34cf2186 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 20 Mar 2024 08:24:33 +0000 Subject: [PATCH 25/35] Made fixed commit for behaviortreee | templated create service function | removed unused warnings Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- .../plugins/include/plugin_test_utils.hpp | 28 +++++-------- .../test/plugins/src/plugin_test_utils.cpp | 41 ------------------- .../test_call_set_bool_service_node.cpp | 12 ++++-- ...st_call_set_led_animation_service_node.cpp | 16 ++++++-- .../test_call_trigger_service_node.cpp | 14 ++++--- .../plugins/test_tick_after_timeout_node.cpp | 8 ++-- 7 files changed, 46 insertions(+), 75 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 4f1a83692..f5b50f855 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,4 +14,4 @@ repositories: behaviortree_ros2: type: git url: https://github.com/BehaviorTree/BehaviorTree.ROS2 - version: humble + version: ce923e19c12d28f4734a41b1442c123f2d4a41d2 diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp index 3e9ecbe16..6c0899f23 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -66,19 +66,17 @@ class PluginTestUtils BT::BehaviorTreeFactory & GetFactory(); - void CreateSetBoolServiceServer( - std::function< - void(std_srvs::srv::SetBool::Request::SharedPtr, std_srvs::srv::SetBool::Response::SharedPtr)> - service_callback); - void CreateTriggerServiceServer( - std::function< - void(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr)> - service_callback); - void CreateSetLEDAnimationServiceServer( - std::function - service_callback); + template + void CreateService( + const std::string & service_name, + std::function, std::shared_ptr)> service_callback) + { + service_server_node_ = std::make_shared("test_node_for_" + service_name); + service_server_node_->create_service(service_name, service_callback); + executor_ = std::make_unique(); + executor_->add_node(service_server_node_); + executor_thread_ = std::make_unique([this]() { executor_->spin(); }); + } private: rclcpp::Node::SharedPtr bt_node_; @@ -88,9 +86,6 @@ class PluginTestUtils rclcpp::Node::SharedPtr service_server_node_; rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; - rclcpp::Service::SharedPtr set_bool_server_; - rclcpp::Service::SharedPtr trigger_server_; - rclcpp::Service::SharedPtr set_led_animation_server_; std::unique_ptr executor_thread_; void spin_executor(); @@ -108,5 +103,4 @@ class PluginTestUtils )"; }; } // namespace panther_manager::plugin_test_utils - #endif // PANTHER_MANAGER_PLUGIN_TEST_UTILS_HPP_ diff --git a/panther_manager/test/plugins/src/plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp index a7711ac7c..128e7f0ee 100644 --- a/panther_manager/test/plugins/src/plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/plugin_test_utils.cpp @@ -84,47 +84,6 @@ PluginTestUtils::~PluginTestUtils() } } -void PluginTestUtils::CreateSetBoolServiceServer( - std::function< - void(std_srvs::srv::SetBool::Request::SharedPtr, std_srvs::srv::SetBool::Response::SharedPtr)> - service_callback) -{ - service_server_node_ = std::make_shared("test_set_bool_service"); - set_bool_server_ = service_server_node_->create_service( - "set_bool", service_callback); - executor_ = std::make_unique(); - executor_->add_node(service_server_node_); - executor_thread_ = std::make_unique([this]() { executor_->spin(); }); -} - -void PluginTestUtils::CreateTriggerServiceServer( - std::function< - void(std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr)> - service_callback) -{ - service_server_node_ = std::make_shared("test_trigger_service"); - trigger_server_ = service_server_node_->create_service( - "trigger", service_callback); - executor_ = std::make_unique(); - executor_->add_node(service_server_node_); - executor_thread_ = std::make_unique([this]() { executor_->spin(); }); -} - -void PluginTestUtils::CreateSetLEDAnimationServiceServer( - std::function - service_callback) -{ - service_server_node_ = std::make_shared("test_set_led_animation_service"); - set_led_animation_server_ = - service_server_node_->create_service( - "set_led_animation", service_callback); - executor_ = std::make_unique(); - executor_->add_node(service_server_node_); - executor_thread_ = std::make_unique([this]() { executor_->spin(); }); -} - void PluginTestUtils::spin_executor() { executor_->spin(); } } // namespace panther_manager::plugin_test_utils diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index 80c906f8e..b59492dd0 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -96,7 +96,9 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckTrueValue); + using std_srvs::srv::SetBool; + test_utils.CreateService( + "test_set_bool_service", ServiceSuccessCallbackCheckTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -111,7 +113,9 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetBoolServiceServer(ServiceSuccessCallbackCheckFalseValue); + using std_srvs::srv::SetBool; + test_utils.CreateService( + "test_set_bool_service", ServiceSuccessCallbackCheckFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -126,7 +130,9 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetBoolServiceServer(ServiceFailedCallback); + using std_srvs::srv::SetBool; + test_utils.CreateService( + "test_set_bool_service", ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 816b41cb7..6d5d6d4ca 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -130,7 +130,9 @@ TEST( test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingTrueValue); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceSuccessCallbackCheckRepeatingTrueValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -148,7 +150,9 @@ TEST( test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckRepeatingFalseValue); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceSuccessCallbackCheckRepeatingFalseValue); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -164,7 +168,9 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetLEDAnimationServiceServer(ServiceSuccessCallbackCheckId5); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceSuccessCallbackCheckId5); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -180,7 +186,9 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateSetLEDAnimationServiceServer(ServiceFailedCallback); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index 553d774fc..70be243c5 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -24,20 +24,18 @@ #include void ServiceFailedCallback( - const std_srvs::srv::Trigger::Request::SharedPtr request, + const std_srvs::srv::Trigger::Request::SharedPtr /* request */, std_srvs::srv::Trigger::Response::SharedPtr response) { - [[maybe_unused]] request; response->message = "Failed callback pass!"; response->success = false; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } void ServiceSuccessCallback( - const std_srvs::srv::Trigger::Request::SharedPtr request, + const std_srvs::srv::Trigger::Request::SharedPtr /* request */, std_srvs::srv::Trigger::Response::SharedPtr response) { - [[maybe_unused]] request; response->message = "Successfully callback pass!"; response->success = true; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); @@ -83,7 +81,9 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) test_utils.CreateTree("CallTriggerService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateTriggerServiceServer(ServiceSuccessCallback); + using std_srvs::srv::Trigger; + test_utils.CreateService( + "test_trigger_service", ServiceSuccessCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -98,7 +98,9 @@ TEST(TestCallTriggerService, wrong_trigger_call_service_failure) test_utils.CreateTree("CallTriggerService", service); auto & tree = test_utils.GetTree(); - test_utils.CreateTriggerServiceServer(ServiceFailedCallback); + using std_srvs::srv::Trigger; + test_utils.CreateService( + "test_trigger_service", ServiceFailedCallback); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index 883c613e5..c28485927 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -27,10 +27,9 @@ inline static std::size_t counter = 0; void CounterIncrease( - const std_srvs::srv::Trigger::Request::SharedPtr request, + const std_srvs::srv::Trigger::Request::SharedPtr /* request */, std_srvs::srv::Trigger::Response::SharedPtr response) { - [[maybe_unused]] request; response->message = "Successfully increased!"; response->success = true; ++counter; @@ -67,7 +66,10 @@ TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); auto & tree = test_utils.GetTree(); - test_utils.CreateTriggerServiceServer(CounterIncrease); + + using std_srvs::srv::Trigger; + test_utils.CreateService( + "test_trigger_service", CounterIncrease); EXPECT_EQ(counter, 0); From 98d6d698218b4ed7974f5833fd846e08d4f0441d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 20 Mar 2024 08:40:33 +0000 Subject: [PATCH 26/35] Added RegisterNode template function Signed-off-by: Jakub Delicat --- .../test/plugins/include/plugin_test_utils.hpp | 15 +++++++++++++++ .../test_call_set_bool_service_node.cpp | 8 ++++++++ ...est_call_set_led_animation_service_node.cpp | 18 ++++++++++++++++++ .../plugins/test_call_trigger_service_node.cpp | 5 +++++ .../test_shutdown_hosts_from_file_node.cpp | 8 ++++++++ .../plugins/test_shutdown_single_host_node.cpp | 6 ++++-- .../test/plugins/test_signal_shutdown_node.cpp | 10 ++++++---- .../plugins/test_tick_after_timeout_node.cpp | 6 ++++++ 8 files changed, 70 insertions(+), 6 deletions(-) diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp index 6c0899f23..f8833ba53 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -78,6 +78,21 @@ class PluginTestUtils executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } + template + void RegisterNodeWithParams(const std::string & node_type_name) + { + BT::RosNodeParams params; + params.nh = bt_node_; + + factory_.registerNodeType(node_type_name, params); + } + + template + void RegisterNodeWithoutParams(const std::string & node_type_name) + { + factory_.registerNodeType(node_type_name); + } + private: rclcpp::Node::SharedPtr bt_node_; BT::BehaviorTreeFactory factory_; diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index b59492dd0..2b88a98b9 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -61,6 +61,7 @@ TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", service); }); } @@ -70,6 +71,7 @@ TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plu std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); } @@ -79,8 +81,10 @@ TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_init std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); test_utils.CreateTree("CallSetBoolService", service); + auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); @@ -92,6 +96,7 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); @@ -109,6 +114,7 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); @@ -126,6 +132,7 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); @@ -144,6 +151,7 @@ TEST(TestCallSetBoolService, wrong_service_value_defined) {"service_name", "set_bool"}, {"data", "wrong_bool"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallSetBoolService"); test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 6d5d6d4ca..374710b2b 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -86,6 +86,8 @@ TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", service); }); } @@ -97,6 +99,8 @@ TEST( {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); EXPECT_THROW( { test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); @@ -110,6 +114,8 @@ TEST( {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -126,6 +132,8 @@ TEST( {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -146,6 +154,8 @@ TEST( {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -164,6 +174,8 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -182,6 +194,8 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -200,6 +214,8 @@ TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "wrong_bool"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); @@ -214,6 +230,8 @@ TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) {"service_name", "set_led_animation"}, {"id", "-5"}, {"param", ""}, {"repeating", "true"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams( + "CallSetLedAnimationService"); test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index 70be243c5..053839f7d 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -46,6 +46,7 @@ TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) std::map service = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); } @@ -55,6 +56,7 @@ TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plug std::map service = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); } @@ -64,6 +66,7 @@ TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initi std::map service = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); test_utils.CreateTree("CallTriggerService", service); auto & tree = test_utils.GetTree(); @@ -77,6 +80,7 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) std::map service = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); test_utils.CreateTree("CallTriggerService", service); auto & tree = test_utils.GetTree(); @@ -94,6 +98,7 @@ TEST(TestCallTriggerService, wrong_trigger_call_service_failure) std::map service = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); test_utils.CreateTree("CallTriggerService", service); auto & tree = test_utils.GetTree(); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index 507bfc7fa..3d1630f5d 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -29,6 +29,8 @@ TEST(TestShutdownHostsFromFile, good_loading_shutdown_hosts_from_file_plugin) const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams( + "ShutdownHostsFromFile"); ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownHostsFromFile", service); }); } @@ -38,6 +40,8 @@ TEST(TestShutdownHostsFromFile, wrong_plugin_name_loading_shutdown_hosts_from_fi const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams( + "ShutdownHostsFromFile"); EXPECT_THROW({ test_utils.CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); } @@ -48,6 +52,8 @@ TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) const std::map service = {{"shutdown_hosts_file", file_path}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams( + "ShutdownHostsFromFile"); test_utils.CreateTree("ShutdownHostsFromFile", service); auto & tree = test_utils.GetTree(); @@ -83,6 +89,8 @@ TEST(TestShutdownHostsFromFile, good_shutdown_hosts_from_file) const std::map service = {{"shutdown_hosts_file", config_file_path}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams( + "ShutdownHostsFromFile"); auto & tree = test_utils.GetTree(); test_utils.CreateTree("ShutdownHostsFromFile", service); diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 1a88ae722..fc296d7cb 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -76,7 +76,7 @@ TEST(TestShutdownSingleHost, good_touch_command) TEST(TestShutdownSingleHost, wrong_command) { std::map service = { - {"command", "command_what_does_not_exists"}, + {"command", "wrong_command"}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, @@ -84,6 +84,7 @@ TEST(TestShutdownSingleHost, wrong_command) {"user", "husarion"}, }; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams("ShutdownSingleHost"); test_utils.CreateTree("ShutdownSingleHost", service); auto & tree = test_utils.GetTree(); @@ -100,9 +101,10 @@ TEST(TestShutdownSingleHost, wrong_user) {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, - {"user", "user_what_does_not_exists"}, + {"user", "wrong_user"}, }; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams("ShutdownSingleHost"); test_utils.CreateTree("ShutdownSingleHost", service); auto & tree = test_utils.GetTree(); diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp index d6b051d7f..5cebb4fba 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -27,6 +27,7 @@ TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) { std::map service = {{"reason", "Test shutdown."}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams("SignalShutdown"); ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); } @@ -44,18 +45,19 @@ TEST(TestSignalShutdown, good_check_reason_blackboard_value) { std::map service = {{"reason", "Test shutdown."}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithoutParams("SignalShutdown"); test_utils.CreateTree("SignalShutdown", service); auto & tree = test_utils.GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); auto blackboard = tree.rootBlackboard(); - auto got_value = blackboard->get>("signal_shutdown"); + auto signal_shutdown_value = blackboard->get>("signal_shutdown"); - EXPECT_EQ(got_value.first, true); - EXPECT_EQ(got_value.second, service["reason"]); + EXPECT_EQ(signal_shutdown_value.first, true); + EXPECT_EQ(signal_shutdown_value.second, service["reason"]); } int main(int argc, char ** argv) diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index c28485927..c6c9fae4b 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -43,6 +43,8 @@ TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) std::map trigger_node = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); + test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); }); @@ -53,6 +55,8 @@ TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) std::map trigger_node = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); + test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); EXPECT_THROW( { test_utils.CreateTree("WrongTriggerService", trigger_node, 0.1); }, BT::RuntimeError); @@ -63,6 +67,8 @@ TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) std::map trigger_node = {{"service_name", "trigger"}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; + test_utils.RegisterNodeWithParams("CallTriggerService"); + test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); auto & tree = test_utils.GetTree(); From 67c31825c962e6eab5945263cd306cbd36694702 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 20 Mar 2024 09:07:15 +0000 Subject: [PATCH 27/35] Changed tests' names to PascalCase | added testing::TempDir() | Starting services when there are wrong parameters Signed-off-by: Jakub Delicat --- panther_lights/test/test_image_animation.cpp | 2 +- .../test_call_set_bool_service_node.cpp | 18 ++++++---- ...st_call_set_led_animation_service_node.cpp | 33 +++++++++--------- .../test_call_trigger_service_node.cpp | 10 +++--- .../test_shutdown_hosts_from_file_node.cpp | 34 ++++++++++--------- .../test_shutdown_single_host_node.cpp | 12 +++---- .../plugins/test_signal_shutdown_node.cpp | 6 ++-- .../plugins/test_tick_after_timeout_node.cpp | 6 ++-- 8 files changed, 64 insertions(+), 57 deletions(-) diff --git a/panther_lights/test/test_image_animation.cpp b/panther_lights/test/test_image_animation.cpp index 6936d0668..bf0136cdf 100644 --- a/panther_lights/test/test_image_animation.cpp +++ b/panther_lights/test/test_image_animation.cpp @@ -70,7 +70,7 @@ class TestImageAnimation : public testing::Test ~TestImageAnimation(); protected: - std::string test_image_path = "/tmp/test_image.png"; + std::string test_image_path = testing::TempDir() + "/test_image.png"; std::unique_ptr animation_; }; diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index 2b88a98b9..bc5955f55 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -56,7 +56,7 @@ void ServiceSuccessCallbackCheckFalseValue( EXPECT_EQ(request->data, false); } -TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) +TEST(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; @@ -66,7 +66,7 @@ TEST(TestCallSetBoolService, good_loading_call_set_bool_service_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", service); }); } -TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plugin) +TEST(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; @@ -76,7 +76,7 @@ TEST(TestCallSetBoolService, wrong_plugin_name_loading_call_set_bool_service_plu EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); } -TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_initialized) +TEST(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialized) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; @@ -91,7 +91,7 @@ TEST(TestCallSetBoolService, wrong_call_set_bool_service_service_server_not_init EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) +TEST(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; @@ -109,7 +109,7 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_true_value) EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value) +TEST(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; @@ -127,7 +127,7 @@ TEST(TestCallSetBoolService, good_set_bool_call_service_success_with_false_value EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) +TEST(TestCallSetBoolService, WrongSetBoolCallServiceFailure) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; @@ -145,7 +145,7 @@ TEST(TestCallSetBoolService, wrong_set_bool_call_service_failure) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetBoolService, wrong_service_value_defined) +TEST(TestCallSetBoolService, WrongServiceValueDefined) { std::map service = { {"service_name", "set_bool"}, {"data", "wrong_bool"}}; @@ -153,6 +153,10 @@ TEST(TestCallSetBoolService, wrong_service_value_defined) panther_manager::plugin_test_utils::PluginTestUtils test_utils; test_utils.RegisterNodeWithParams("CallSetBoolService"); + using std_srvs::srv::SetBool; + test_utils.CreateService( + "test_set_bool_service", ServiceFailedCallback); + test_utils.CreateTree("CallSetBoolService", service); auto & tree = test_utils.GetTree(); diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 374710b2b..5ca302586 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -80,7 +80,7 @@ void ServiceSuccessCallbackCheckId5( EXPECT_EQ(request->animation.id, 5u); } -TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service_plugin) +TEST(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -92,8 +92,7 @@ TEST(TestCallSetLedAnimationService, good_loading_call_set_led_animation_service ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", service); }); } -TEST( - TestCallSetLedAnimationService, wrong_plugin_name_loading_call_set_led_animation_service_plugin) +TEST(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimationServicePlugin) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -106,9 +105,7 @@ TEST( { test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); } -TEST( - TestCallSetLedAnimationService, - wrong_call_set_led_animation_service_service_server_not_initialized) +TEST(TestCallSetLedAnimationService, WrongCallSetLedAnimationServiceServiceServerNotInitialized) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -124,9 +121,7 @@ TEST( EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST( - TestCallSetLedAnimationService, - good_set_led_animation_call_service_success_with_true_repeating_value) +TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithTrueRepeatingValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -146,9 +141,7 @@ TEST( EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST( - TestCallSetLedAnimationService, - good_set_led_animation_call_service_success_with_false_repeating_value) +TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithFalseRepeatingValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; @@ -168,7 +161,7 @@ TEST( EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success_with_5_id_value) +TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith_5IdValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; @@ -188,7 +181,7 @@ TEST(TestCallSetLedAnimationService, good_set_led_animation_call_service_success EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failure) +TEST(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -208,7 +201,7 @@ TEST(TestCallSetLedAnimationService, wrong_set_led_animation_call_service_failur EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) +TEST(TestCallSetLedAnimationService, WrongRepeatingServiceValueDefined) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "wrong_bool"}}; @@ -220,11 +213,15 @@ TEST(TestCallSetLedAnimationService, wrong_repeating_service_value_defined) test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceFailedCallback); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) +TEST(TestCallSetLedAnimationService, WrongIdServiceValueDefined) { std::map service = { {"service_name", "set_led_animation"}, {"id", "-5"}, {"param", ""}, {"repeating", "true"}}; @@ -236,6 +233,10 @@ TEST(TestCallSetLedAnimationService, wrong_id_service_value_defined) test_utils.CreateTree("CallSetLedAnimationService", service); auto & tree = test_utils.GetTree(); + using panther_msgs::srv::SetLEDAnimation; + test_utils.CreateService( + "test_set_led_animation_service", ServiceFailedCallback); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index 053839f7d..6887d7690 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -41,7 +41,7 @@ void ServiceSuccessCallback( RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } -TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) +TEST(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; @@ -51,7 +51,7 @@ TEST(TestCallTriggerService, good_loading_call_trigger_service_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); } -TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plugin) +TEST(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; @@ -61,7 +61,7 @@ TEST(TestCallTriggerService, wrong_plugin_name_loading_call_trigger_service_plug EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); } -TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initialized) +TEST(TestCallTriggerService, WrongCallTriggerServiceServiceServerNotInitialized) { std::map service = {{"service_name", "trigger"}}; @@ -75,7 +75,7 @@ TEST(TestCallTriggerService, wrong_call_trigger_service_service_server_not_initi EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallTriggerService, good_trigger_call_service_success) +TEST(TestCallTriggerService, GoodTriggerCallServiceSuccess) { std::map service = {{"service_name", "trigger"}}; @@ -93,7 +93,7 @@ TEST(TestCallTriggerService, good_trigger_call_service_success) EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallTriggerService, wrong_trigger_call_service_failure) +TEST(TestCallTriggerService, WrongTriggerCallServiceFailure) { std::map service = {{"service_name", "trigger"}}; diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index 3d1630f5d..294df047c 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -24,7 +24,7 @@ #include #include -TEST(TestShutdownHostsFromFile, good_loading_shutdown_hosts_from_file_plugin) +TEST(TestShutdownHostsFromFile, GoodLoadingShutdownHostsFromFilePlugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; @@ -35,7 +35,7 @@ TEST(TestShutdownHostsFromFile, good_loading_shutdown_hosts_from_file_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownHostsFromFile", service); }); } -TEST(TestShutdownHostsFromFile, wrong_plugin_name_loading_shutdown_hosts_from_file_plugin) +TEST(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; @@ -46,9 +46,10 @@ TEST(TestShutdownHostsFromFile, wrong_plugin_name_loading_shutdown_hosts_from_fi EXPECT_THROW({ test_utils.CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); } -TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) +TEST(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) { - const std::string file_path = "/tmp/test_wrong_cannot_find_file_shutdown_hosts_from_file"; + const std::string file_path = testing::TempDir() + + "/test_wrong_cannot_find_file_shutdown_hosts_from_file"; const std::map service = {{"shutdown_hosts_file", file_path}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; @@ -61,29 +62,30 @@ TEST(TestShutdownHostsFromFile, wrong_cannot_find_file_shutdown_hosts_from_file) EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); } -TEST(TestShutdownHostsFromFile, good_shutdown_hosts_from_file) +TEST(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) { - const std::string config_file_path = - "/tmp/test_panther_manager_good_shutdown_hosts_from_file_config"; - const std::string test_file_path = "/tmp/test_panther_manager_good_shutdown_hosts_from_file"; + const std::string config_file_path = testing::TempDir() + + "/test_panther_manager_good_shutdown_hosts_from_file_config"; + const std::string test_file_path = testing::TempDir() + + "/test_panther_manager_good_shutdown_hosts_from_file"; std::filesystem::remove(test_file_path); std::filesystem::remove(config_file_path); EXPECT_FALSE(std::filesystem::exists(test_file_path)); EXPECT_FALSE(std::filesystem::exists(config_file_path)); - YAML::Node yaml; - yaml["hosts"][0]["ip"] = "localhost"; - yaml["hosts"][0]["username"] = "husarion"; - yaml["hosts"][0]["port"] = 22; - yaml["hosts"][0]["command"] = "touch " + test_file_path; - yaml["hosts"][0]["timeout"] = 5.0; - yaml["hosts"][0]["ping_for_success"] = false; + YAML::Node shutdown_host_desc; + shutdown_host_desc["hosts"][0]["ip"] = "localhost"; + shutdown_host_desc["hosts"][0]["username"] = "husarion"; + shutdown_host_desc["hosts"][0]["port"] = 22; + shutdown_host_desc["hosts"][0]["command"] = "touch " + test_file_path; + shutdown_host_desc["hosts"][0]["timeout"] = 5.0; + shutdown_host_desc["hosts"][0]["ping_for_success"] = false; std::fstream config_file; YAML::Emitter emitter(config_file); config_file.open(config_file_path, std::ios::app); - emitter << yaml; + emitter << shutdown_host_desc; config_file.close(); diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index fc296d7cb..bf98c803b 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -24,7 +24,7 @@ #include #include -TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) +TEST(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, @@ -35,7 +35,7 @@ TEST(TestShutdownSingleHost, good_loading_shutdown_single_host_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownSingleHost", service); }); } -TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plugin) +TEST(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, @@ -47,9 +47,9 @@ TEST(TestShutdownSingleHost, wrong_plugin_name_loading_shutdown_single_host_plug EXPECT_THROW({ test_utils.CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); } -TEST(TestShutdownSingleHost, good_touch_command) +TEST(TestShutdownSingleHost, GoodTouchCommand) { - std::string file_path = "/tmp/test_panther_manager_good_touch_command"; + std::string file_path = testing::TempDir() + "/test_panther_manager_good_touch_command"; std::filesystem::remove(file_path); EXPECT_FALSE(std::filesystem::exists(file_path)); @@ -73,7 +73,7 @@ TEST(TestShutdownSingleHost, good_touch_command) std::filesystem::remove(file_path); } -TEST(TestShutdownSingleHost, wrong_command) +TEST(TestShutdownSingleHost, WrongCommand) { std::map service = { {"command", "wrong_command"}, @@ -93,7 +93,7 @@ TEST(TestShutdownSingleHost, wrong_command) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestShutdownSingleHost, wrong_user) +TEST(TestShutdownSingleHost, WrongUser) { std::map service = { {"command", "echo Hello World!"}, diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp index 5cebb4fba..983ce96d2 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -23,7 +23,7 @@ #include #include -TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) +TEST(TestSignalShutdown, GoodLoadingSignalShutdownPlugin) { std::map service = {{"reason", "Test shutdown."}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; @@ -32,7 +32,7 @@ TEST(TestSignalShutdown, good_loading_signal_shutdown_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); } -TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) +TEST(TestSignalShutdown, WrongPluginNameLoadingSignalShutdownPlugin) { std::map service = {}; @@ -41,7 +41,7 @@ TEST(TestSignalShutdown, wrong_plugin_name_loading_signal_shutdown_plugin) EXPECT_THROW({ test_utils.CreateTree("WrongSignalShutdown", service); }, BT::RuntimeError); } -TEST(TestSignalShutdown, good_check_reason_blackboard_value) +TEST(TestSignalShutdown, GoodCheckReasonBlackboardValue) { std::map service = {{"reason", "Test shutdown."}}; panther_manager::plugin_test_utils::PluginTestUtils test_utils; diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index c6c9fae4b..d467c3948 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -38,7 +38,7 @@ void CounterIncrease( response->message << " Counter value: " << counter); } -TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) +TEST(TestTickAfterTimeout, GoodLoadingTickAfterTimeoutPlugin) { std::map trigger_node = {{"service_name", "trigger"}}; @@ -50,7 +50,7 @@ TEST(TestTickAfterTimeout, good_loading_tick_after_timeout_plugin) ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); }); } -TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) +TEST(TestTickAfterTimeout, WrongPluginNameLoadingTickAfterTimeoutPlugin) { std::map trigger_node = {{"service_name", "trigger"}}; @@ -62,7 +62,7 @@ TEST(TestTickAfterTimeout, wrong_plugin_name_loading_tick_after_timeout_plugin) { test_utils.CreateTree("WrongTriggerService", trigger_node, 0.1); }, BT::RuntimeError); } -TEST(TestTickAfterTimeout, good_tick_after_timeout_plugin_service_calls) +TEST(TestTickAfterTimeout, GoodTickAfterTimeoutPluginServiceCalls) { std::map trigger_node = {{"service_name", "trigger"}}; From f59ab95b9d14f3d3a176aa8195c56435037a5c96 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 20 Mar 2024 22:22:36 +0000 Subject: [PATCH 28/35] fixed tests Signed-off-by: Jakub Delicat --- panther_manager/package.xml | 1 + .../plugins/include/plugin_test_utils.hpp | 12 +- .../test/plugins/src/plugin_test_utils.cpp | 43 ++--- .../test_call_set_bool_service_node.cpp | 121 +++++++------ ...st_call_set_led_animation_service_node.cpp | 161 ++++++++++-------- .../test_call_trigger_service_node.cpp | 69 ++++---- .../test_shutdown_hosts_from_file_node.cpp | 38 ++--- .../test_shutdown_single_host_node.cpp | 40 ++--- .../plugins/test_signal_shutdown_node.cpp | 26 +-- .../plugins/test_tick_after_timeout_node.cpp | 75 -------- 10 files changed, 272 insertions(+), 314 deletions(-) diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 8d6d0cff1..3f82cd537 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -20,6 +20,7 @@ iputils-ping libboost-dev libssh-dev + panther_msgs panther_utils rclcpp rclcpp_action diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp index f8833ba53..c76639c45 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -47,12 +48,12 @@ struct BehaviorTreePluginDescription std::map params; }; -class PluginTestUtils +class PluginTestUtils : public testing::Test { public: - PluginTestUtils(); + virtual void SetUp() override final; - ~PluginTestUtils(); + virtual void TearDown() override final; std::string BuildBehaviorTree( const std::string & plugin_name, const std::map & service, @@ -72,7 +73,7 @@ class PluginTestUtils std::function, std::shared_ptr)> service_callback) { service_server_node_ = std::make_shared("test_node_for_" + service_name); - service_server_node_->create_service(service_name, service_callback); + service = service_server_node_->create_service(service_name, service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); @@ -93,7 +94,7 @@ class PluginTestUtils factory_.registerNodeType(node_type_name); } -private: +protected: rclcpp::Node::SharedPtr bt_node_; BT::BehaviorTreeFactory factory_; BT::Tree tree_; @@ -101,6 +102,7 @@ class PluginTestUtils rclcpp::Node::SharedPtr service_server_node_; rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; + rclcpp::ServiceBase::SharedPtr service; std::unique_ptr executor_thread_; void spin_executor(); diff --git a/panther_manager/test/plugins/src/plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp index 128e7f0ee..061048680 100644 --- a/panther_manager/test/plugins/src/plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/plugin_test_utils.cpp @@ -17,6 +17,22 @@ namespace panther_manager::plugin_test_utils { +void PluginTestUtils::SetUp() +{ + rclcpp::init(0, nullptr); + bt_node_ = std::make_shared("test_panther_manager_node"); +} + +void PluginTestUtils::TearDown() +{ + bt_node_.reset(); + rclcpp::shutdown(); + if (executor_thread_) { + executor_.reset(); + executor_thread_->join(); + } +} + std::string PluginTestUtils::BuildBehaviorTree( const std::string & plugin_name, const std::map & service, double tick_after_timeout) @@ -57,33 +73,6 @@ BT::Tree & PluginTestUtils::GetTree() { return tree_; } BT::BehaviorTreeFactory & PluginTestUtils::GetFactory() { return factory_; } -PluginTestUtils::PluginTestUtils() -{ - rclcpp::init(0, nullptr); - bt_node_ = std::make_shared("test_panther_manager_node"); - BT::RosNodeParams params; - params.nh = bt_node_; - - factory_.registerNodeType("CallSetBoolService", params); - factory_.registerNodeType("CallTriggerService", params); - factory_.registerNodeType( - "CallSetLedAnimationService", params); - factory_.registerNodeType("SignalShutdown"); - factory_.registerNodeType("ShutdownSingleHost"); - factory_.registerNodeType("ShutdownHostsFromFile"); - factory_.registerNodeType("TickAfterTimeout"); -} - -PluginTestUtils::~PluginTestUtils() -{ - bt_node_.reset(); - rclcpp::shutdown(); - if (executor_thread_) { - executor_.reset(); - executor_thread_->join(); - } -} - void PluginTestUtils::spin_executor() { executor_->spin(); } } // namespace panther_manager::plugin_test_utils diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index bc5955f55..9b1b218a0 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -21,8 +21,23 @@ #include #include +class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + void ServiceFailedCallback( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response); + + void ServiceSuccessCallbackCheckTrueValue( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response); -void ServiceFailedCallback( + void ServiceSuccessCallbackCheckFalseValue( + const std_srvs::srv::SetBool::Request::SharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response); +}; + +void TestCallSetBoolService::ServiceFailedCallback( const std_srvs::srv::SetBool::Request::SharedPtr request, std_srvs::srv::SetBool::Response::SharedPtr response) { @@ -32,7 +47,7 @@ void ServiceFailedCallback( rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); } -void ServiceSuccessCallbackCheckTrueValue( +void TestCallSetBoolService::ServiceSuccessCallbackCheckTrueValue( const std_srvs::srv::SetBool::Request::SharedPtr request, std_srvs::srv::SetBool::Response::SharedPtr response) { @@ -44,7 +59,7 @@ void ServiceSuccessCallbackCheckTrueValue( EXPECT_EQ(request->data, true); } -void ServiceSuccessCallbackCheckFalseValue( +void TestCallSetBoolService::ServiceSuccessCallbackCheckFalseValue( const std_srvs::srv::SetBool::Request::SharedPtr request, std_srvs::srv::SetBool::Response::SharedPtr response) { @@ -56,111 +71,113 @@ void ServiceSuccessCallbackCheckFalseValue( EXPECT_EQ(request->data, false); } -TEST(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) +TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); - ASSERT_NO_THROW({ test_utils.CreateTree("CallSetBoolService", service); }); + ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service); }); } -TEST(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin) +TEST_F(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); - EXPECT_THROW({ test_utils.CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongCallSetBoolService", service); }, BT::RuntimeError); } -TEST(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialized) +TEST_F(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialized) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); + RegisterNodeWithParams("CallSetBoolService"); - test_utils.CreateTree("CallSetBoolService", service); + CreateTree("CallSetBoolService", service); - auto & tree = test_utils.GetTree(); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue) +TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; + using std::placeholders::_1; + using std::placeholders::_2; + using std_srvs::srv::SetBool; + CreateService( + "set_bool", + std::bind(&TestCallSetBoolService::ServiceSuccessCallbackCheckTrueValue, this, _1, _2)); - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); - - test_utils.CreateTree("CallSetBoolService", service); - auto & tree = test_utils.GetTree(); + RegisterNodeWithParams("CallSetBoolService"); - using std_srvs::srv::SetBool; - test_utils.CreateService( - "test_set_bool_service", ServiceSuccessCallbackCheckTrueValue); + CreateTree("CallSetBoolService", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue) +TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); + using std::placeholders::_1; + using std::placeholders::_2; + using std_srvs::srv::SetBool; + CreateService( + "set_bool", + std::bind(&TestCallSetBoolService::ServiceSuccessCallbackCheckFalseValue, this, _1, _2)); - test_utils.CreateTree("CallSetBoolService", service); - auto & tree = test_utils.GetTree(); + RegisterNodeWithParams("CallSetBoolService"); - using std_srvs::srv::SetBool; - test_utils.CreateService( - "test_set_bool_service", ServiceSuccessCallbackCheckFalseValue); + CreateTree("CallSetBoolService", service); + auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetBoolService, WrongSetBoolCallServiceFailure) +TEST_F(TestCallSetBoolService, WrongSetBoolCallServiceFailure) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); + using std::placeholders::_1; + using std::placeholders::_2; + using std_srvs::srv::SetBool; + CreateService( + "set_bool", std::bind(&TestCallSetBoolService::ServiceFailedCallback, this, _1, _2)); - test_utils.CreateTree("CallSetBoolService", service); - auto & tree = test_utils.GetTree(); + RegisterNodeWithParams("CallSetBoolService"); - using std_srvs::srv::SetBool; - test_utils.CreateService( - "test_set_bool_service", ServiceFailedCallback); + CreateTree("CallSetBoolService", service); + auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetBoolService, WrongServiceValueDefined) +TEST_F(TestCallSetBoolService, WrongServiceValueDefined) { std::map service = { {"service_name", "set_bool"}, {"data", "wrong_bool"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallSetBoolService"); - + using std::placeholders::_1; + using std::placeholders::_2; using std_srvs::srv::SetBool; - test_utils.CreateService( - "test_set_bool_service", ServiceFailedCallback); + CreateService( + "set_bool", std::bind(&TestCallSetBoolService::ServiceFailedCallback, this, _1, _2)); - test_utils.CreateTree("CallSetBoolService", service); - auto & tree = test_utils.GetTree(); + RegisterNodeWithParams("CallSetBoolService"); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + CreateTree("CallSetBoolService", service); + auto & tree = GetTree(); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index 5ca302586..aa744ea70 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -22,7 +22,27 @@ #include #include -void ServiceFailedCallback( +class TestCallSetLedAnimationService : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + void ServiceFailedCallback( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); + + void ServiceSuccessCallbackCheckRepeatingTrueValue( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); + + void ServiceSuccessCallbackCheckRepeatingFalseValue( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); + + void ServiceSuccessCallbackCheckId5( + const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); +}; + +void TestCallSetLedAnimationService::ServiceFailedCallback( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { @@ -35,7 +55,7 @@ void ServiceFailedCallback( << " repeating: " << request->repeating); } -void ServiceSuccessCallbackCheckRepeatingTrueValue( +void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingTrueValue( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { @@ -50,7 +70,7 @@ void ServiceSuccessCallbackCheckRepeatingTrueValue( EXPECT_EQ(request->repeating, true); } -void ServiceSuccessCallbackCheckRepeatingFalseValue( +void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingFalseValue( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { @@ -65,7 +85,7 @@ void ServiceSuccessCallbackCheckRepeatingFalseValue( EXPECT_EQ(request->repeating, false); } -void ServiceSuccessCallbackCheckId5( +void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckId5( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) { @@ -80,162 +100,165 @@ void ServiceSuccessCallbackCheckId5( EXPECT_EQ(request->animation.id, 5u); } -TEST(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) +TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - ASSERT_NO_THROW({ test_utils.CreateTree("CallSetLedAnimationService", service); }); + ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service); }); } -TEST(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimationServicePlugin) +TEST_F(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimationServicePlugin) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - EXPECT_THROW( - { test_utils.CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongCallSetLedAnimationService", service); }, BT::RuntimeError); } -TEST(TestCallSetLedAnimationService, WrongCallSetLedAnimationServiceServiceServerNotInitialized) +TEST_F(TestCallSetLedAnimationService, WrongCallSetLedAnimationServiceServiceServerNotInitialized) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithTrueRepeatingValue) +TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithTrueRepeatingValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceSuccessCallbackCheckRepeatingTrueValue); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind( + &TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingTrueValue, this, _1, + _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithFalseRepeatingValue) +TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWithFalseRepeatingValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "false"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceSuccessCallbackCheckRepeatingFalseValue); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind( + &TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingFalseValue, this, _1, + _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith_5IdValue) +TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith_5IdValue) { std::map service = { {"service_name", "set_led_animation"}, {"id", "5"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceSuccessCallbackCheckId5); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind(&TestCallSetLedAnimationService::ServiceSuccessCallbackCheckId5, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure) +TEST_F(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceFailedCallback); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetLedAnimationService, WrongRepeatingServiceValueDefined) +TEST_F(TestCallSetLedAnimationService, WrongRepeatingServiceValueDefined) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "wrong_bool"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceFailedCallback); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallSetLedAnimationService, WrongIdServiceValueDefined) +TEST_F(TestCallSetLedAnimationService, WrongIdServiceValueDefined) { std::map service = { {"service_name", "set_led_animation"}, {"id", "-5"}, {"param", ""}, {"repeating", "true"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams( - "CallSetLedAnimationService"); + RegisterNodeWithParams("CallSetLedAnimationService"); - test_utils.CreateTree("CallSetLedAnimationService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallSetLedAnimationService", service); + auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - test_utils.CreateService( - "test_set_led_animation_service", ServiceFailedCallback); + using std::placeholders::_1; + using std::placeholders::_2; + CreateService( + "set_led_animation", + std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index 6887d7690..f02e21d7b 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -23,7 +23,18 @@ #include #include -void ServiceFailedCallback( +class TestCallTriggerService : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + void ServiceFailedCallback( + const std_srvs::srv::Trigger::Request::SharedPtr /* request */, + std_srvs::srv::Trigger::Response::SharedPtr response); + void ServiceSuccessCallback( + const std_srvs::srv::Trigger::Request::SharedPtr /* request */, + std_srvs::srv::Trigger::Response::SharedPtr response); +}; + +void TestCallTriggerService::ServiceFailedCallback( const std_srvs::srv::Trigger::Request::SharedPtr /* request */, std_srvs::srv::Trigger::Response::SharedPtr response) { @@ -32,7 +43,7 @@ void ServiceFailedCallback( RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } -void ServiceSuccessCallback( +void TestCallTriggerService::ServiceSuccessCallback( const std_srvs::srv::Trigger::Request::SharedPtr /* request */, std_srvs::srv::Trigger::Response::SharedPtr response) { @@ -41,71 +52,71 @@ void ServiceSuccessCallback( RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } -TEST(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) +TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); - ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", service); }); + ASSERT_NO_THROW({ CreateTree("CallTriggerService", service); }); } -TEST(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin) +TEST_F(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); - EXPECT_THROW({ test_utils.CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongCallTriggerService", service); }, BT::RuntimeError); } -TEST(TestCallTriggerService, WrongCallTriggerServiceServiceServerNotInitialized) +TEST_F(TestCallTriggerService, WrongCallTriggerServiceServiceServerNotInitialized) { std::map service = {{"service_name", "trigger"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); - test_utils.CreateTree("CallTriggerService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallTriggerService", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestCallTriggerService, GoodTriggerCallServiceSuccess) +TEST_F(TestCallTriggerService, GoodTriggerCallServiceSuccess) { std::map service = {{"service_name", "trigger"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); - test_utils.CreateTree("CallTriggerService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallTriggerService", service); + auto & tree = GetTree(); + using std::placeholders::_1; + using std::placeholders::_2; using std_srvs::srv::Trigger; - test_utils.CreateService( - "test_trigger_service", ServiceSuccessCallback); + + CreateService( + "trigger", std::bind(&TestCallTriggerService::ServiceSuccessCallback, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST(TestCallTriggerService, WrongTriggerCallServiceFailure) +TEST_F(TestCallTriggerService, WrongTriggerCallServiceFailure) { std::map service = {{"service_name", "trigger"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); + RegisterNodeWithParams("CallTriggerService"); - test_utils.CreateTree("CallTriggerService", service); - auto & tree = test_utils.GetTree(); + CreateTree("CallTriggerService", service); + auto & tree = GetTree(); + using std::placeholders::_1; + using std::placeholders::_2; using std_srvs::srv::Trigger; - test_utils.CreateService( - "test_trigger_service", ServiceFailedCallback); + CreateService( + "trigger", std::bind(&TestCallTriggerService::ServiceFailedCallback, this, _1, _2)); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index 294df047c..a7829b34e 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -24,45 +24,41 @@ #include #include -TEST(TestShutdownHostsFromFile, GoodLoadingShutdownHostsFromFilePlugin) +typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownHostsFromFile; + +TEST_F(TestShutdownHostsFromFile, GoodLoadingShutdownHostsFromFilePlugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams( - "ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); - ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownHostsFromFile", service); }); + ASSERT_NO_THROW({ CreateTree("ShutdownHostsFromFile", service); }); } -TEST(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlugin) +TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlugin) { const std::map service = {{"shutdown_hosts_file", "dummy_file"}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams( - "ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); - EXPECT_THROW({ test_utils.CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongShutdownHostsFromFile", service); }, BT::RuntimeError); } -TEST(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) +TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) { const std::string file_path = testing::TempDir() + "/test_wrong_cannot_find_file_shutdown_hosts_from_file"; const std::map service = {{"shutdown_hosts_file", file_path}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams( - "ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); - test_utils.CreateTree("ShutdownHostsFromFile", service); - auto & tree = test_utils.GetTree(); + CreateTree("ShutdownHostsFromFile", service); + auto & tree = GetTree(); EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); } -TEST(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) +TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) { const std::string config_file_path = testing::TempDir() + "/test_panther_manager_good_shutdown_hosts_from_file_config"; @@ -90,12 +86,10 @@ TEST(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) config_file.close(); const std::map service = {{"shutdown_hosts_file", config_file_path}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams( - "ShutdownHostsFromFile"); + RegisterNodeWithoutParams("ShutdownHostsFromFile"); - auto & tree = test_utils.GetTree(); - test_utils.CreateTree("ShutdownHostsFromFile", service); + auto & tree = GetTree(); + CreateTree("ShutdownHostsFromFile", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index bf98c803b..78b937f86 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -24,30 +24,29 @@ #include #include -TEST(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) +typedef panther_manager::plugin_test_utils::PluginTestUtils TestShutdownSingleHost; + +TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - ASSERT_NO_THROW({ test_utils.CreateTree("ShutdownSingleHost", service); }); + ASSERT_NO_THROW({ CreateTree("ShutdownSingleHost", service); }); } -TEST(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) +TEST_F(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - - EXPECT_THROW({ test_utils.CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); } -TEST(TestShutdownSingleHost, GoodTouchCommand) +TEST_F(TestShutdownSingleHost, GoodTouchCommand) { std::string file_path = testing::TempDir() + "/test_panther_manager_good_touch_command"; std::filesystem::remove(file_path); @@ -61,10 +60,9 @@ TEST(TestShutdownSingleHost, GoodTouchCommand) {"timeout", "5.0"}, {"user", "husarion"}, }; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.GetTree(); + CreateTree("ShutdownSingleHost", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -73,7 +71,7 @@ TEST(TestShutdownSingleHost, GoodTouchCommand) std::filesystem::remove(file_path); } -TEST(TestShutdownSingleHost, WrongCommand) +TEST_F(TestShutdownSingleHost, WrongCommand) { std::map service = { {"command", "wrong_command"}, @@ -83,17 +81,16 @@ TEST(TestShutdownSingleHost, WrongCommand) {"timeout", "0.2"}, {"user", "husarion"}, }; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); - test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.GetTree(); + CreateTree("ShutdownSingleHost", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST(TestShutdownSingleHost, WrongUser) +TEST_F(TestShutdownSingleHost, WrongUser) { std::map service = { {"command", "echo Hello World!"}, @@ -103,12 +100,11 @@ TEST(TestShutdownSingleHost, WrongUser) {"timeout", "5.0"}, {"user", "wrong_user"}, }; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams("ShutdownSingleHost"); + RegisterNodeWithoutParams("ShutdownSingleHost"); - test_utils.CreateTree("ShutdownSingleHost", service); - auto & tree = test_utils.GetTree(); - test_utils.CreateTree("ShutdownSingleHost", service); + CreateTree("ShutdownSingleHost", service); + auto & tree = GetTree(); + CreateTree("ShutdownSingleHost", service); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/test_signal_shutdown_node.cpp index 983ce96d2..0e03510a9 100644 --- a/panther_manager/test/plugins/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/test_signal_shutdown_node.cpp @@ -23,32 +23,32 @@ #include #include -TEST(TestSignalShutdown, GoodLoadingSignalShutdownPlugin) +typedef panther_manager::plugin_test_utils::PluginTestUtils TestSignalShutdown; + +TEST_F(TestSignalShutdown, GoodLoadingSignalShutdownPlugin) { std::map service = {{"reason", "Test shutdown."}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams("SignalShutdown"); - ASSERT_NO_THROW({ test_utils.CreateTree("SignalShutdown", service); }); + RegisterNodeWithoutParams("SignalShutdown"); + + ASSERT_NO_THROW({ CreateTree("SignalShutdown", service); }); } -TEST(TestSignalShutdown, WrongPluginNameLoadingSignalShutdownPlugin) +TEST_F(TestSignalShutdown, WrongPluginNameLoadingSignalShutdownPlugin) { std::map service = {}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - - EXPECT_THROW({ test_utils.CreateTree("WrongSignalShutdown", service); }, BT::RuntimeError); + EXPECT_THROW({ CreateTree("WrongSignalShutdown", service); }, BT::RuntimeError); } -TEST(TestSignalShutdown, GoodCheckReasonBlackboardValue) +TEST_F(TestSignalShutdown, GoodCheckReasonBlackboardValue) { std::map service = {{"reason", "Test shutdown."}}; - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithoutParams("SignalShutdown"); - test_utils.CreateTree("SignalShutdown", service); - auto & tree = test_utils.GetTree(); + RegisterNodeWithoutParams("SignalShutdown"); + + CreateTree("SignalShutdown", service); + auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index d467c3948..2478b6cbc 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -24,81 +24,6 @@ #include #include -inline static std::size_t counter = 0; - -void CounterIncrease( - const std_srvs::srv::Trigger::Request::SharedPtr /* request */, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - response->message = "Successfully increased!"; - response->success = true; - ++counter; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_tick_after_timeout_plugin"), - response->message << " Counter value: " << counter); -} - -TEST(TestTickAfterTimeout, GoodLoadingTickAfterTimeoutPlugin) -{ - std::map trigger_node = {{"service_name", "trigger"}}; - - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); - test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); - - test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); - ASSERT_NO_THROW({ test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); }); -} - -TEST(TestTickAfterTimeout, WrongPluginNameLoadingTickAfterTimeoutPlugin) -{ - std::map trigger_node = {{"service_name", "trigger"}}; - - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); - test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); - - EXPECT_THROW( - { test_utils.CreateTree("WrongTriggerService", trigger_node, 0.1); }, BT::RuntimeError); -} - -TEST(TestTickAfterTimeout, GoodTickAfterTimeoutPluginServiceCalls) -{ - std::map trigger_node = {{"service_name", "trigger"}}; - - panther_manager::plugin_test_utils::PluginTestUtils test_utils; - test_utils.RegisterNodeWithParams("CallTriggerService"); - test_utils.RegisterNodeWithoutParams("TickAfterTimeout"); - - test_utils.CreateTree("CallTriggerService", trigger_node, 0.1); - auto & tree = test_utils.GetTree(); - - using std_srvs::srv::Trigger; - test_utils.CreateService( - "test_trigger_service", CounterIncrease); - - EXPECT_EQ(counter, 0); - - auto status = BT::NodeStatus::IDLE; - - for (std::size_t i = 2; i < 5; ++i) { - auto start = std::chrono::high_resolution_clock::now(); - int64_t duration; - while ((status = tree.tickOnce()) != BT::NodeStatus::FAILURE) { - auto end = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(end - start).count(); - EXPECT_LT(duration, 101); - if (counter == i and status == BT::NodeStatus::SUCCESS) { - break; - } - } - - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - EXPECT_EQ(duration, 100); - } - EXPECT_EQ(counter, 4); -} - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From e1b040a01c71ffefeee9671f0cbb143043c3fb5e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 21 Mar 2024 12:04:26 +0000 Subject: [PATCH 29/35] fixed tick after timeout node Signed-off-by: Jakub Delicat --- .../panther_manager/plugins/shutdown_host.hpp | 1 - .../plugins/include/plugin_test_utils.hpp | 8 ++-- .../test/plugins/src/plugin_test_utils.cpp | 15 ++----- .../plugins/test_tick_after_timeout_node.cpp | 40 +++++++++++++++++++ 4 files changed, 46 insertions(+), 18 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index c3235bbcc..54fc83693 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -41,7 +41,6 @@ enum class ShutdownHostState { class ShutdownHost { public: - // default constructor ShutdownHost() : ip_(""), user_(""), diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp index c76639c45..b6a554e76 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -55,13 +55,11 @@ class PluginTestUtils : public testing::Test virtual void TearDown() override final; - std::string BuildBehaviorTree( - const std::string & plugin_name, const std::map & service, - double tick_after_timeout); + virtual std::string BuildBehaviorTree( + const std::string & plugin_name, const std::map & service); void CreateTree( - const std::string & plugin_name, const std::map & service, - double tick_after_timeout = std::numeric_limits::quiet_NaN()); + const std::string & plugin_name, const std::map & service); BT::Tree & GetTree(); diff --git a/panther_manager/test/plugins/src/plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp index 061048680..0fa821232 100644 --- a/panther_manager/test/plugins/src/plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/plugin_test_utils.cpp @@ -34,15 +34,11 @@ void PluginTestUtils::TearDown() } std::string PluginTestUtils::BuildBehaviorTree( - const std::string & plugin_name, const std::map & service, - double tick_after_timeout) + const std::string & plugin_name, const std::map & service) { std::stringstream bt; bt << tree_header_ << std::endl; - if (not std::isnan(tick_after_timeout)) { - bt << "\t\t\t" << std::endl; - } bt << "\t\t\t\t<" << plugin_name << " "; @@ -52,20 +48,15 @@ std::string PluginTestUtils::BuildBehaviorTree( bt << " />" << std::endl; - if (not std::isnan(tick_after_timeout)) { - bt << "\t\t\t" << std::endl; - } - bt << tree_footer_; return bt.str(); } void PluginTestUtils::CreateTree( - const std::string & plugin_name, const std::map & service, - double tick_after_timeout) + const std::string & plugin_name, const std::map & service) { - auto xml_text = BuildBehaviorTree(plugin_name, service, tick_after_timeout); + auto xml_text = BuildBehaviorTree(plugin_name, service); tree_ = factory_.createTreeFromText(xml_text); } diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index 2478b6cbc..268d0ebfc 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -24,6 +24,46 @@ #include #include +class TestTickAfterTimeout : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + virtual std::string BuildBehaviorTree( + const std::string & plugin_name, const std::map & service) override; +}; + +std::string TestTickAfterTimeout::BuildBehaviorTree( + const std::string & /* plugin_name */, const std::map & /* service */ +) +{ + std::stringstream bt; + + bt << tree_header_ << std::endl; + bt << "\t\t\t" << std::endl; + + bt << "\t\t\t\t" << std::endl; + + bt << "\t\t\t" << std::endl; + + bt << tree_footer_; + + return bt.str(); +} + +TEST_F(TestTickAfterTimeout, GoodTickAfterTimeout) +{ + RegisterNodeWithoutParams("TickAfterTimeout"); + + CreateTree("", {}); + auto & tree = GetTree(); + + for (std::size_t i = 0; i < 10; ++i) { + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + if (status != BT::NodeStatus::SUCCESS && status != BT::NodeStatus::SKIPPED) { + FAIL() << "Bad status of behavior tree."; + } + } +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 7084b464d6f1a6706772a4e3b7e4ac590c66f2df Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 22 Mar 2024 10:43:36 +0000 Subject: [PATCH 30/35] fixed user to username | added test_shutdown_host Signed-off-by: Jakub Delicat --- panther_manager/CMakeLists.txt | 8 ++ .../action/shutdown_single_host_node.hpp | 2 +- .../panther_manager/plugins/shutdown_host.hpp | 1 + panther_manager/package.xml | 1 + .../action/shutdown_hosts_from_file_node.cpp | 4 +- .../action/shutdown_single_host_node.cpp | 2 +- .../test/plugins/test_shutdown_host.cpp | 106 ++++++++++++++++++ .../test_shutdown_hosts_from_file_node.cpp | 8 +- .../test/plugins/test_shutdown_hosts_node.cpp | 52 +++++++++ .../test_shutdown_single_host_node.cpp | 19 ++-- 10 files changed, 187 insertions(+), 16 deletions(-) create mode 100644 panther_manager/test/plugins/test_shutdown_host.cpp create mode 100644 panther_manager/test/plugins/test_shutdown_hosts_node.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 18405ad8c..cb5ec9958 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -108,6 +108,14 @@ if(BUILD_TESTING) test/plugins/test_tick_after_timeout_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_tick_after_timeout_node) + ament_add_gtest(${PROJECT_NAME}_test_shutdown_host + test/plugins/test_shutdown_host.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_host) + + ament_add_gtest(${PROJECT_NAME}_test_shutdown_hosts_node + test/plugins/test_shutdown_hosts_node.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_node) + foreach(bt_node_test ${plugin_tests}) target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils) ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS}) diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 01880dd04..6aee8448a 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -39,7 +39,7 @@ class ShutdownSingleHost : public ShutdownHosts { return { BT::InputPort("ip", "ip of the host to shutdown"), - BT::InputPort("user", "user to log into while executing shutdown command"), + BT::InputPort("username", "user to log into while executing shutdown command"), BT::InputPort("port", "SSH communication port"), BT::InputPort("command", "command to execute on shutdown"), BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index 54fc83693..eb2c6beb9 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -103,6 +103,7 @@ class ShutdownHost case ShutdownHostState::RESPONSE_RECEIVED: state_ = ShutdownHostState::PINGING; break; + case ShutdownHostState::PINGING: if (ping_for_success_ ? !is_available() : true) { state_ = ShutdownHostState::SUCCESS; diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 3f82cd537..cc5ca4232 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -17,6 +17,7 @@ ament_cmake behaviortree_cpp + behaviortree_ros2 iputils-ping libboost-dev libssh-dev diff --git a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp index f9c5a4c56..bfe4edf98 100644 --- a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -52,14 +52,14 @@ void ShutdownHostsFromFile::update_hosts(std::vector(host, "ip"); - auto user = panther_utils::GetYAMLKeyValue(host, "user"); + auto username = panther_utils::GetYAMLKeyValue(host, "username"); auto port = panther_utils::GetYAMLKeyValue(host, "port", 22); auto command = panther_utils::GetYAMLKeyValue( host, "command", "sudo shutdown now"); auto timeout = panther_utils::GetYAMLKeyValue(host, "timeout", 5.0); auto ping_for_success = panther_utils::GetYAMLKeyValue(host, "ping_for_success", true); hosts.push_back( - std::make_shared(ip, user, port, command, timeout, ping_for_success)); + std::make_shared(ip, username, port, command, timeout, ping_for_success)); } } catch (const std::runtime_error & e) { throw BT::RuntimeError("[" + this->name() + "]: " + e.what()); diff --git a/panther_manager/plugins/action/shutdown_single_host_node.cpp b/panther_manager/plugins/action/shutdown_single_host_node.cpp index 9ec250f09..320bdb395 100644 --- a/panther_manager/plugins/action/shutdown_single_host_node.cpp +++ b/panther_manager/plugins/action/shutdown_single_host_node.cpp @@ -34,7 +34,7 @@ void ShutdownSingleHost::update_hosts(std::vector> } std::string user; - if (!getInput("user", user) || user == "") { + if (!getInput("username", user) || user == "") { throw(BT::RuntimeError("[", this->name(), "] Failed to get input [user]")); } diff --git a/panther_manager/test/plugins/test_shutdown_host.cpp b/panther_manager/test/plugins/test_shutdown_host.cpp new file mode 100644 index 000000000..dcb1ea26c --- /dev/null +++ b/panther_manager/test/plugins/test_shutdown_host.cpp @@ -0,0 +1,106 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 + +class TestShutdownHost : public testing::Test +{ +public: + void Create( + const std::string ip, const std::string user, const int port, const std::string command, + const float timeout, const bool ping_for_success); + bool IsAvailable(); + void Call(); + panther_manager::ShutdownHostState GetState() const; + std::string GetResponse() const; + +private: + std::unique_ptr shutdown_host; +}; + +void TestShutdownHost::Create( + const std::string ip, const std::string user, const int port, const std::string command, + const float timeout, const bool ping_for_success) +{ + shutdown_host = std::make_unique( + ip, user, port, command, timeout, ping_for_success); +} + +bool TestShutdownHost::IsAvailable() { return shutdown_host->is_available(); } + +void TestShutdownHost::Call() { shutdown_host->call(); } + +panther_manager::ShutdownHostState TestShutdownHost::GetState() const +{ + return shutdown_host->get_state(); +} + +std::string TestShutdownHost::GetResponse() const { return shutdown_host->get_response(); } + +TEST_F(TestShutdownHost, GoodCheckIsAvailable) +{ + Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, true); + EXPECT_TRUE(IsAvailable()); + EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); +} + +TEST_F(TestShutdownHost, WrongCheckIsAvailable) +{ + Create("1.45.23.26", "husarion", 22, "echo HelloWorld", 0.1, true); + EXPECT_FALSE(IsAvailable()); +} + +TEST_F(TestShutdownHost, GoodCommandExecute) +{ + Create("127.0.0.1", "husarion", 22, "echo HelloWorld", 0.1, false); + + ASSERT_TRUE(IsAvailable()); + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + Call(); + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED); + // Wait for response + while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) { + Call(); + } + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::RESPONSE_RECEIVED); + + const auto response = GetResponse(); + EXPECT_EQ(response, "HelloWorld\n"); + + Call(); + + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::PINGING); + Call(); + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::SUCCESS); +} + +TEST_F(TestShutdownHost, WrongHostPing) +{ + Create("1.45.23.26", "husarion", 22, "echo HelloWorld", 0.1, true); + + EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + Call(); + EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::SKIPPED); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index a7829b34e..36548dbc6 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -46,8 +46,8 @@ TEST_F(TestShutdownHostsFromFile, WrongPluginNameLoadingShutdownHostsFromFilePlu TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) { - const std::string file_path = testing::TempDir() + - "/test_wrong_cannot_find_file_shutdown_hosts_from_file"; + const std::string file_path = + testing::TempDir() + "test_panther_manager_wrong_cannot_find_file_shutdown_hosts_from_file"; const std::map service = {{"shutdown_hosts_file", file_path}}; RegisterNodeWithoutParams("ShutdownHostsFromFile"); @@ -61,9 +61,9 @@ TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) { const std::string config_file_path = testing::TempDir() + - "/test_panther_manager_good_shutdown_hosts_from_file_config"; + "test_panther_manager_good_shutdown_hosts_from_file_config"; const std::string test_file_path = testing::TempDir() + - "/test_panther_manager_good_shutdown_hosts_from_file"; + "test_panther_manager_good_shutdown_hosts_from_file"; std::filesystem::remove(test_file_path); std::filesystem::remove(config_file_path); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp new file mode 100644 index 000000000..3019f07c6 --- /dev/null +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -0,0 +1,52 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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 +#include + +class ShutdownHostsNodeDuplicated : public panther_manager::ShutdownHosts +{ +public: + ShutdownHostsNodeDuplicated(const std::string & name, const BT::NodeConfig & conf) + : panther_manager::ShutdownHosts(name, conf) + { + } + + static BT::PortsList providedPorts() { return {}; } + +private: + virtual void update_hosts( + std::vector> & hosts) override final; +}; + +void ShutdownHostsNodeDuplicated::update_hosts( + std::vector> & hosts) +{ + hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); + hosts.emplace_back(std::make_shared("localhost", "husarion")); + hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); +} + +// TODO: Create TESTs completely + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 78b937f86..e09d1cc28 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -30,9 +30,11 @@ TEST_F(TestShutdownSingleHost, GoodLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, - {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, + {"port", "22"}, {"timeout", "5.0"}, {"username", "husarion"}, }; + RegisterNodeWithoutParams("ShutdownSingleHost"); + ASSERT_NO_THROW({ CreateTree("ShutdownSingleHost", service); }); } @@ -40,27 +42,28 @@ TEST_F(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) { std::map service = { {"command", "pwd"}, {"ip", "localhost"}, {"ping_for_success", "false"}, - {"port", "22"}, {"timeout", "5.0"}, {"user", "husarion"}, + {"port", "22"}, {"timeout", "5.0"}, {"username", "husarion"}, }; + RegisterNodeWithoutParams("ShutdownSingleHost"); EXPECT_THROW({ CreateTree("WrongShutdownSingleHost", service); }, BT::RuntimeError); } TEST_F(TestShutdownSingleHost, GoodTouchCommand) { - std::string file_path = testing::TempDir() + "/test_panther_manager_good_touch_command"; + std::string file_path = testing::TempDir() + "test_panther_manager_good_touch_command"; std::filesystem::remove(file_path); EXPECT_FALSE(std::filesystem::exists(file_path)); - + std::cout << "Path: " << file_path << std::endl; std::map service = { {"command", "touch " + file_path}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, - {"user", "husarion"}, + {"username", "husarion"}, }; - + RegisterNodeWithoutParams("ShutdownSingleHost"); CreateTree("ShutdownSingleHost", service); auto & tree = GetTree(); @@ -79,7 +82,7 @@ TEST_F(TestShutdownSingleHost, WrongCommand) {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "0.2"}, - {"user", "husarion"}, + {"username", "husarion"}, }; RegisterNodeWithoutParams("ShutdownSingleHost"); @@ -98,7 +101,7 @@ TEST_F(TestShutdownSingleHost, WrongUser) {"ping_for_success", "false"}, {"port", "22"}, {"timeout", "5.0"}, - {"user", "wrong_user"}, + {"username", "wrong_user"}, }; RegisterNodeWithoutParams("ShutdownSingleHost"); From 40a5d444573fecc9544520338d610ddca6dff107 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 22 Mar 2024 12:43:07 +0000 Subject: [PATCH 31/35] Added all suggestions Signed-off-by: Jakub Delicat --- .../action/shutdown_hosts_from_file_node.hpp | 2 +- .../action/shutdown_single_host_node.hpp | 2 +- .../plugins/shutdown_hosts_node.hpp | 12 +- .../action/call_set_bool_service_node.cpp | 2 +- .../action/shutdown_hosts_from_file_node.cpp | 25 +-- .../action/shutdown_single_host_node.cpp | 21 ++- .../plugins/action/signal_shutdown_node.cpp | 2 +- .../plugins/include/plugin_test_utils.hpp | 27 ++-- .../test/plugins/src/plugin_test_utils.cpp | 14 +- .../test_call_set_bool_service_node.cpp | 82 ++++------ ...st_call_set_led_animation_service_node.cpp | 142 ++++++------------ .../test_call_trigger_service_node.cpp | 43 +++--- .../test_shutdown_hosts_from_file_node.cpp | 4 +- .../test/plugins/test_shutdown_hosts_node.cpp | 5 +- .../test_shutdown_single_host_node.cpp | 27 +++- .../plugins/test_tick_after_timeout_node.cpp | 26 +++- 16 files changed, 197 insertions(+), 239 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index a1132bf1f..6cfa65629 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -46,7 +46,7 @@ class ShutdownHostsFromFile : public ShutdownHosts } private: - void update_hosts(std::vector> & hosts) override; + bool update_hosts(std::vector> & hosts) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 6aee8448a..47b6a2f10 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -49,7 +49,7 @@ class ShutdownSingleHost : public ShutdownHosts } private: - void update_hosts(std::vector> & hosts) override; + bool update_hosts(std::vector> & hosts) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 5f67d12de..b457c83e6 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -47,7 +47,7 @@ class ShutdownHosts : public BT::StatefulActionNode virtual ~ShutdownHosts() = default; // method to be implemented by user - virtual void update_hosts(std::vector> & hosts) = 0; + virtual bool update_hosts(std::vector> & hosts) = 0; // method that can be overridden by user virtual BT::NodeStatus post_process() @@ -61,6 +61,9 @@ class ShutdownHosts : public BT::StatefulActionNode std::vector const get_failed_hosts() { return this->failed_hosts_; } +protected: + std::shared_ptr logger_; + private: std::size_t check_host_index_ = 0; std::vector> hosts_; @@ -68,11 +71,14 @@ class ShutdownHosts : public BT::StatefulActionNode std::vector skipped_hosts_; std::vector succeeded_hosts_; std::vector failed_hosts_; - std::shared_ptr logger_; BT::NodeStatus onStart() { - update_hosts(this->hosts_); + if (!update_hosts(this->hosts_)) { + RCLCPP_ERROR_STREAM(*this->logger_, "Cannot update hosts!"); + return BT::NodeStatus::FAILURE; + } + remove_duplicate_hosts(this->hosts_); if (this->hosts_.size() <= 0) { RCLCPP_ERROR_STREAM(*this->logger_, "Hosts list is empty! Check configuration!"); diff --git a/panther_manager/plugins/action/call_set_bool_service_node.cpp b/panther_manager/plugins/action/call_set_bool_service_node.cpp index 634b9f7e0..cbbae2a3b 100644 --- a/panther_manager/plugins/action/call_set_bool_service_node.cpp +++ b/panther_manager/plugins/action/call_set_bool_service_node.cpp @@ -19,7 +19,7 @@ namespace panther_manager bool CallSetBoolService::setRequest(typename Request::SharedPtr & request) { if (!getInput("data", request->data)) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to get input [data]"); + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "Failed to get input [data]"); return false; } return true; diff --git a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp index bfe4edf98..5c2abf9db 100644 --- a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -28,20 +28,22 @@ namespace panther_manager { -void ShutdownHostsFromFile::update_hosts(std::vector> & hosts) +bool ShutdownHostsFromFile::update_hosts(std::vector> & hosts) { std::string shutdown_hosts_file; if ( !getInput("shutdown_hosts_file", shutdown_hosts_file) || shutdown_hosts_file == "") { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [shutdown_hosts_file]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [shutdown_hosts_file]"); + return false; } YAML::Node shutdown_hosts; try { shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); } catch (const YAML::Exception & e) { - throw BT::RuntimeError("[" + this->name() + "] Error loading YAML file: " + e.what()); + RCLCPP_ERROR_STREAM(*this->logger_, " Error loading YAML file: " << e.what()); + return false; } try { @@ -51,19 +53,22 @@ void ShutdownHostsFromFile::update_hosts(std::vector(host, "ip"); - auto username = panther_utils::GetYAMLKeyValue(host, "username"); - auto port = panther_utils::GetYAMLKeyValue(host, "port", 22); - auto command = panther_utils::GetYAMLKeyValue( + const auto ip = panther_utils::GetYAMLKeyValue(host, "ip"); + const auto username = panther_utils::GetYAMLKeyValue(host, "username"); + const auto port = panther_utils::GetYAMLKeyValue(host, "port", 22); + const auto command = panther_utils::GetYAMLKeyValue( host, "command", "sudo shutdown now"); - auto timeout = panther_utils::GetYAMLKeyValue(host, "timeout", 5.0); - auto ping_for_success = panther_utils::GetYAMLKeyValue(host, "ping_for_success", true); + const auto timeout = panther_utils::GetYAMLKeyValue(host, "timeout", 5.0); + const auto ping_for_success = panther_utils::GetYAMLKeyValue( + host, "ping_for_success", true); hosts.push_back( std::make_shared(ip, username, port, command, timeout, ping_for_success)); } } catch (const std::runtime_error & e) { - throw BT::RuntimeError("[" + this->name() + "]: " + e.what()); + RCLCPP_ERROR_STREAM(*this->logger_, e.what()); + return false; } + return true; } } // namespace panther_manager diff --git a/panther_manager/plugins/action/shutdown_single_host_node.cpp b/panther_manager/plugins/action/shutdown_single_host_node.cpp index 320bdb395..2b060572b 100644 --- a/panther_manager/plugins/action/shutdown_single_host_node.cpp +++ b/panther_manager/plugins/action/shutdown_single_host_node.cpp @@ -26,40 +26,47 @@ namespace panther_manager { -void ShutdownSingleHost::update_hosts(std::vector> & hosts) +bool ShutdownSingleHost::update_hosts(std::vector> & hosts) { std::string ip; if (!getInput("ip", ip) || ip == "") { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [ip]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [ip]"); + return false; } std::string user; if (!getInput("username", user) || user == "") { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [user]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [user]"); + return false; } unsigned port; if (!getInput("port", port)) { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [port]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [port]"); + return false; } std::string command; if (!getInput("command", command) || command == "") { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [command]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [command]"); + return false; } float timeout; if (!getInput("timeout", timeout)) { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [timeout]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [timeout]"); + return false; } bool ping_for_success; if (!getInput("ping_for_success", ping_for_success)) { - throw(BT::RuntimeError("[", this->name(), "] Failed to get input [ping_for_success]")); + RCLCPP_ERROR_STREAM(*this->logger_, "Failed to get input [ping_for_success]"); + return false; } hosts.push_back( std::make_shared(ip, user, port, command, timeout, ping_for_success)); + return true; } } // namespace panther_manager diff --git a/panther_manager/plugins/action/signal_shutdown_node.cpp b/panther_manager/plugins/action/signal_shutdown_node.cpp index 0f1fa190f..9d90c4a42 100644 --- a/panther_manager/plugins/action/signal_shutdown_node.cpp +++ b/panther_manager/plugins/action/signal_shutdown_node.cpp @@ -24,7 +24,7 @@ namespace panther_manager BT::NodeStatus SignalShutdown::tick() { - auto reason = this->getInput("reason").value(); + const auto reason = this->getInput("reason").value(); std::pair signal_shutdown; signal_shutdown.first = true; diff --git a/panther_manager/test/plugins/include/plugin_test_utils.hpp b/panther_manager/test/plugins/include/plugin_test_utils.hpp index b6a554e76..4db32116f 100644 --- a/panther_manager/test/plugins/include/plugin_test_utils.hpp +++ b/panther_manager/test/plugins/include/plugin_test_utils.hpp @@ -51,45 +51,46 @@ struct BehaviorTreePluginDescription class PluginTestUtils : public testing::Test { public: - virtual void SetUp() override final; - - virtual void TearDown() override final; + PluginTestUtils(); + ~PluginTestUtils(); virtual std::string BuildBehaviorTree( - const std::string & plugin_name, const std::map & service); + const std::string & plugin_name, const std::map & bb_ports); void CreateTree( - const std::string & plugin_name, const std::map & service); + const std::string & plugin_name, const std::map & bb_ports); BT::Tree & GetTree(); BT::BehaviorTreeFactory & GetFactory(); - template + template void CreateService( const std::string & service_name, - std::function, std::shared_ptr)> service_callback) + std::function< + void(const typename ServiceT::Request::SharedPtr, typename ServiceT::Response::SharedPtr)> + service_callback) { service_server_node_ = std::make_shared("test_node_for_" + service_name); - service = service_server_node_->create_service(service_name, service_callback); + service = service_server_node_->create_service(service_name, service_callback); executor_ = std::make_unique(); executor_->add_node(service_server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); } - template + template void RegisterNodeWithParams(const std::string & node_type_name) { BT::RosNodeParams params; params.nh = bt_node_; - factory_.registerNodeType(node_type_name, params); + factory_.registerNodeType(node_type_name, params); } - template + template void RegisterNodeWithoutParams(const std::string & node_type_name) { - factory_.registerNodeType(node_type_name); + factory_.registerNodeType(node_type_name); } protected: @@ -103,7 +104,7 @@ class PluginTestUtils : public testing::Test rclcpp::ServiceBase::SharedPtr service; std::unique_ptr executor_thread_; - void spin_executor(); + void SpinExecutor(); const std::string tree_header_ = R"( diff --git a/panther_manager/test/plugins/src/plugin_test_utils.cpp b/panther_manager/test/plugins/src/plugin_test_utils.cpp index 0fa821232..2d86992e2 100644 --- a/panther_manager/test/plugins/src/plugin_test_utils.cpp +++ b/panther_manager/test/plugins/src/plugin_test_utils.cpp @@ -17,13 +17,13 @@ namespace panther_manager::plugin_test_utils { -void PluginTestUtils::SetUp() +PluginTestUtils::PluginTestUtils() { rclcpp::init(0, nullptr); bt_node_ = std::make_shared("test_panther_manager_node"); } -void PluginTestUtils::TearDown() +PluginTestUtils::~PluginTestUtils() { bt_node_.reset(); rclcpp::shutdown(); @@ -34,7 +34,7 @@ void PluginTestUtils::TearDown() } std::string PluginTestUtils::BuildBehaviorTree( - const std::string & plugin_name, const std::map & service) + const std::string & plugin_name, const std::map & bb_ports) { std::stringstream bt; @@ -42,7 +42,7 @@ std::string PluginTestUtils::BuildBehaviorTree( bt << "\t\t\t\t<" << plugin_name << " "; - for (auto const & [key, value] : service) { + for (auto const & [key, value] : bb_ports) { bt << key << "=\"" << value << "\" "; } @@ -54,9 +54,9 @@ std::string PluginTestUtils::BuildBehaviorTree( } void PluginTestUtils::CreateTree( - const std::string & plugin_name, const std::map & service) + const std::string & plugin_name, const std::map & bb_ports) { - auto xml_text = BuildBehaviorTree(plugin_name, service); + auto xml_text = BuildBehaviorTree(plugin_name, bb_ports); tree_ = factory_.createTreeFromText(xml_text); } @@ -64,6 +64,6 @@ BT::Tree & PluginTestUtils::GetTree() { return tree_; } BT::BehaviorTreeFactory & PluginTestUtils::GetFactory() { return factory_; } -void PluginTestUtils::spin_executor() { executor_->spin(); } +void PluginTestUtils::SpinExecutor() { executor_->spin(); } } // namespace panther_manager::plugin_test_utils diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index 9b1b218a0..65beb0cfb 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -24,51 +24,21 @@ class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils { public: - void ServiceFailedCallback( + void ServiceCallback( const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response); - - void ServiceSuccessCallbackCheckTrueValue( - const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response); - - void ServiceSuccessCallbackCheckFalseValue( - const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response); + std_srvs::srv::SetBool::Response::SharedPtr response, const bool success, + const bool expected_value); }; -void TestCallSetBoolService::ServiceFailedCallback( - const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) -{ - response->message = "Failed callback pass!"; - response->success = false; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); -} - -void TestCallSetBoolService::ServiceSuccessCallbackCheckTrueValue( - const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) -{ - response->message = "Successfully callback pass!"; - response->success = true; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); - - EXPECT_EQ(request->data, true); -} - -void TestCallSetBoolService::ServiceSuccessCallbackCheckFalseValue( +void TestCallSetBoolService::ServiceCallback( const std_srvs::srv::SetBool::Request::SharedPtr request, - std_srvs::srv::SetBool::Response::SharedPtr response) + std_srvs::srv::SetBool::Response::SharedPtr response, bool success, bool expected_value) { - response->message = "Successfully callback pass!"; - response->success = true; + response->message = success ? "Successfully callback pass!" : "Failed callback pass!"; + response->success = success; + EXPECT_EQ(request->data, expected_value); RCLCPP_INFO_STREAM( rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); - - EXPECT_EQ(request->data, false); } TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) @@ -106,12 +76,13 @@ TEST_F(TestCallSetBoolService, WrongCallSetBoolServiceServiceServerNotInitialize TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithTrueValue) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; - using std::placeholders::_1; - using std::placeholders::_2; + using std_srvs::srv::SetBool; - CreateService( + CreateService( "set_bool", - std::bind(&TestCallSetBoolService::ServiceSuccessCallbackCheckTrueValue, this, _1, _2)); + [&](const SetBool::Request::SharedPtr request, SetBool::Response::SharedPtr response) { + ServiceCallback(request, response, true, true); + }); RegisterNodeWithParams("CallSetBoolService"); @@ -126,12 +97,12 @@ TEST_F(TestCallSetBoolService, GoodSetBoolCallServiceSuccessWithFalseValue) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - using std::placeholders::_1; - using std::placeholders::_2; using std_srvs::srv::SetBool; - CreateService( + CreateService( "set_bool", - std::bind(&TestCallSetBoolService::ServiceSuccessCallbackCheckFalseValue, this, _1, _2)); + [&](const SetBool::Request::SharedPtr request, SetBool::Response::SharedPtr response) { + ServiceCallback(request, response, true, false); + }); RegisterNodeWithParams("CallSetBoolService"); @@ -146,11 +117,12 @@ TEST_F(TestCallSetBoolService, WrongSetBoolCallServiceFailure) { std::map service = {{"service_name", "set_bool"}, {"data", "false"}}; - using std::placeholders::_1; - using std::placeholders::_2; using std_srvs::srv::SetBool; - CreateService( - "set_bool", std::bind(&TestCallSetBoolService::ServiceFailedCallback, this, _1, _2)); + CreateService( + "set_bool", + [&](const SetBool::Request::SharedPtr request, SetBool::Response::SharedPtr response) { + ServiceCallback(request, response, false, false); + }); RegisterNodeWithParams("CallSetBoolService"); @@ -166,12 +138,12 @@ TEST_F(TestCallSetBoolService, WrongServiceValueDefined) std::map service = { {"service_name", "set_bool"}, {"data", "wrong_bool"}}; - using std::placeholders::_1; - using std::placeholders::_2; using std_srvs::srv::SetBool; - CreateService( - "set_bool", std::bind(&TestCallSetBoolService::ServiceFailedCallback, this, _1, _2)); - + CreateService( + "set_bool", + [&](const SetBool::Request::SharedPtr request, SetBool::Response::SharedPtr response) { + ServiceCallback(request, response, false, true); + }); RegisterNodeWithParams("CallSetBoolService"); CreateTree("CallSetBoolService", service); diff --git a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp index aa744ea70..544d88756 100644 --- a/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_led_animation_service_node.cpp @@ -25,79 +25,27 @@ class TestCallSetLedAnimationService : public panther_manager::plugin_test_utils::PluginTestUtils { public: - void ServiceFailedCallback( + void ServiceCallback( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); - - void ServiceSuccessCallbackCheckRepeatingTrueValue( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); - - void ServiceSuccessCallbackCheckRepeatingFalseValue( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); - - void ServiceSuccessCallbackCheckId5( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response); + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response, const bool success, + const std::size_t id, const bool repeating); }; -void TestCallSetLedAnimationService::ServiceFailedCallback( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) -{ - response->message = "Failed callback pass!"; - response->success = false; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); -} - -void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingTrueValue( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) -{ - response->message = "Successfully callback pass!"; - response->success = true; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); - - EXPECT_EQ(request->repeating, true); -} - -void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingFalseValue( - const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) -{ - response->message = "Successfully callback pass!"; - response->success = true; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("test_set_led_animation_plugin"), - response->message << " success: " << response->success << " id: " << request->animation.id - << " param: " << request->animation.param - << " repeating: " << request->repeating); - - EXPECT_EQ(request->repeating, false); -} - -void TestCallSetLedAnimationService::ServiceSuccessCallbackCheckId5( +void TestCallSetLedAnimationService::ServiceCallback( const panther_msgs::srv::SetLEDAnimation::Request::SharedPtr request, - panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response) + panther_msgs::srv::SetLEDAnimation::Response::SharedPtr response, const bool success, + const std::size_t id, const bool repeating) { - response->message = "Successfully callback pass!"; - response->success = true; + response->message = success ? "Successfully callback pass!" : "Failed callback pass!"; + response->success = success; RCLCPP_INFO_STREAM( rclcpp::get_logger("test_set_led_animation_plugin"), response->message << " success: " << response->success << " id: " << request->animation.id << " param: " << request->animation.param << " repeating: " << request->repeating); - EXPECT_EQ(request->animation.id, 5u); + EXPECT_EQ(request->animation.id, id); + EXPECT_EQ(request->repeating, repeating); } TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) @@ -145,14 +93,13 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind( - &TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingTrueValue, this, _1, - _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, true, 0, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } @@ -168,14 +115,13 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind( - &TestCallSetLedAnimationService::ServiceSuccessCallbackCheckRepeatingFalseValue, this, _1, - _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, true, 0, false); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } @@ -191,12 +137,13 @@ TEST_F(TestCallSetLedAnimationService, GoodSetLedAnimationCallServiceSuccessWith auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind(&TestCallSetLedAnimationService::ServiceSuccessCallbackCheckId5, this, _1, _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, true, 5, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } @@ -212,12 +159,13 @@ TEST_F(TestCallSetLedAnimationService, WrongSetLedAnimationCallServiceFailure) auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, false, 0, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } @@ -233,12 +181,13 @@ TEST_F(TestCallSetLedAnimationService, WrongRepeatingServiceValueDefined) auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, true, 0, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } @@ -254,12 +203,13 @@ TEST_F(TestCallSetLedAnimationService, WrongIdServiceValueDefined) auto & tree = GetTree(); using panther_msgs::srv::SetLEDAnimation; - using std::placeholders::_1; - using std::placeholders::_2; - CreateService( - "set_led_animation", - std::bind(&TestCallSetLedAnimationService::ServiceFailedCallback, this, _1, _2)); + CreateService( + "set_led_animation", [&]( + const SetLEDAnimation::Request::SharedPtr request, + SetLEDAnimation::Response::SharedPtr response) { + ServiceCallback(request, response, true, 0, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } diff --git a/panther_manager/test/plugins/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/test_call_trigger_service_node.cpp index f02e21d7b..c158f1d3f 100644 --- a/panther_manager/test/plugins/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/test_call_trigger_service_node.cpp @@ -26,29 +26,17 @@ class TestCallTriggerService : public panther_manager::plugin_test_utils::PluginTestUtils { public: - void ServiceFailedCallback( + void ServiceCallback( const std_srvs::srv::Trigger::Request::SharedPtr /* request */, - std_srvs::srv::Trigger::Response::SharedPtr response); - void ServiceSuccessCallback( - const std_srvs::srv::Trigger::Request::SharedPtr /* request */, - std_srvs::srv::Trigger::Response::SharedPtr response); + std_srvs::srv::Trigger::Response::SharedPtr response, const bool success); }; -void TestCallTriggerService::ServiceFailedCallback( +void TestCallTriggerService::ServiceCallback( const std_srvs::srv::Trigger::Request::SharedPtr /* request */, - std_srvs::srv::Trigger::Response::SharedPtr response) + std_srvs::srv::Trigger::Response::SharedPtr response, const bool success) { - response->message = "Failed callback pass!"; - response->success = false; - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); -} - -void TestCallTriggerService::ServiceSuccessCallback( - const std_srvs::srv::Trigger::Request::SharedPtr /* request */, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - response->message = "Successfully callback pass!"; - response->success = true; + response->message = success ? "Successfully callback pass!" : "Failed callback pass!"; + response->success = success; RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } @@ -92,12 +80,13 @@ TEST_F(TestCallTriggerService, GoodTriggerCallServiceSuccess) CreateTree("CallTriggerService", service); auto & tree = GetTree(); - using std::placeholders::_1; - using std::placeholders::_2; using std_srvs::srv::Trigger; - CreateService( - "trigger", std::bind(&TestCallTriggerService::ServiceSuccessCallback, this, _1, _2)); + CreateService( + "trigger", + [&](const Trigger::Request::SharedPtr request, Trigger::Response::SharedPtr response) { + ServiceCallback(request, response, true); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); @@ -112,11 +101,13 @@ TEST_F(TestCallTriggerService, WrongTriggerCallServiceFailure) CreateTree("CallTriggerService", service); auto & tree = GetTree(); - using std::placeholders::_1; - using std::placeholders::_2; using std_srvs::srv::Trigger; - CreateService( - "trigger", std::bind(&TestCallTriggerService::ServiceFailedCallback, this, _1, _2)); + + CreateService( + "trigger", + [&](const Trigger::Request::SharedPtr request, Trigger::Response::SharedPtr response) { + ServiceCallback(request, response, false); + }); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp index 36548dbc6..31e35de9f 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_from_file_node.cpp @@ -55,7 +55,8 @@ TEST_F(TestShutdownHostsFromFile, WrongCannotFindFileShutdownHostsFromFile) CreateTree("ShutdownHostsFromFile", service); auto & tree = GetTree(); - EXPECT_THROW({ tree.tickWhileRunning(std::chrono::milliseconds(100)); }, BT::RuntimeError); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); } TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) @@ -74,6 +75,7 @@ TEST_F(TestShutdownHostsFromFile, GoodShutdownHostsFromFile) shutdown_host_desc["hosts"][0]["ip"] = "localhost"; shutdown_host_desc["hosts"][0]["username"] = "husarion"; shutdown_host_desc["hosts"][0]["port"] = 22; + shutdown_host_desc["hosts"][0]["command"] = "touch " + test_file_path; shutdown_host_desc["hosts"][0]["timeout"] = 5.0; shutdown_host_desc["hosts"][0]["ping_for_success"] = false; diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index 3019f07c6..ff1f072cc 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -30,16 +30,17 @@ class ShutdownHostsNodeDuplicated : public panther_manager::ShutdownHosts static BT::PortsList providedPorts() { return {}; } private: - virtual void update_hosts( + virtual bool update_hosts( std::vector> & hosts) override final; }; -void ShutdownHostsNodeDuplicated::update_hosts( +bool ShutdownHostsNodeDuplicated::update_hosts( std::vector> & hosts) { hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); hosts.emplace_back(std::make_shared("localhost", "husarion")); hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); + return true; } // TODO: Create TESTs completely diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index e09d1cc28..58f141f0a 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -51,12 +51,11 @@ TEST_F(TestShutdownSingleHost, WrongPluginNameLoadingShutdownSingleHostPlugin) TEST_F(TestShutdownSingleHost, GoodTouchCommand) { - std::string file_path = testing::TempDir() + "test_panther_manager_good_touch_command"; - std::filesystem::remove(file_path); - EXPECT_FALSE(std::filesystem::exists(file_path)); - std::cout << "Path: " << file_path << std::endl; + std::string test_file_path = testing::TempDir() + "test_panther_manager_good_touch_command"; + std::filesystem::remove(test_file_path); + EXPECT_FALSE(std::filesystem::exists(test_file_path)); std::map service = { - {"command", "touch " + file_path}, + {"command", "touch " + test_file_path}, {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, @@ -69,9 +68,23 @@ TEST_F(TestShutdownSingleHost, GoodTouchCommand) auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::SUCCESS); - EXPECT_TRUE(std::filesystem::exists(file_path)); + EXPECT_TRUE(std::filesystem::exists(test_file_path)); - std::filesystem::remove(file_path); + std::filesystem::remove(test_file_path); +} + +TEST_F(TestShutdownSingleHost, Timeout) +{ + std::map service = { + {"command", "sleep 0.5"}, {"ip", "localhost"}, {"ping_for_success", "false"}, + {"port", "22"}, {"timeout", "0.1"}, {"username", "husarion"}, + }; + RegisterNodeWithoutParams("ShutdownSingleHost"); + CreateTree("ShutdownSingleHost", service); + auto & tree = GetTree(); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); } TEST_F(TestShutdownSingleHost, WrongCommand) diff --git a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp index 268d0ebfc..6dfc61512 100644 --- a/panther_manager/test/plugins/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/test_tick_after_timeout_node.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -52,16 +51,27 @@ std::string TestTickAfterTimeout::BuildBehaviorTree( TEST_F(TestTickAfterTimeout, GoodTickAfterTimeout) { RegisterNodeWithoutParams("TickAfterTimeout"); - CreateTree("", {}); auto & tree = GetTree(); - for (std::size_t i = 0; i < 10; ++i) { - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - if (status != BT::NodeStatus::SUCCESS && status != BT::NodeStatus::SKIPPED) { - FAIL() << "Bad status of behavior tree."; - } - } + // node has just started the timeout should not be reached + auto status = tree.tickOnce(); + EXPECT_EQ(BT::NodeStatus::SKIPPED, status); + + // sleep for timeout duration + // then the node should return child status - AlwaysSuccess + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + status = tree.tickOnce(); + EXPECT_EQ(BT::NodeStatus::SUCCESS, status); + + // tick again to check if timeout was reset + status = tree.tickOnce(); + EXPECT_EQ(BT::NodeStatus::SKIPPED, status); + + // check again if timeout is correctly evaluated + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + status = tree.tickOnce(); + EXPECT_EQ(BT::NodeStatus::SUCCESS, status); } int main(int argc, char ** argv) From ffe5c93b504f4d8b7c4a435c8989f1ef98db4690 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 25 Mar 2024 09:00:01 +0000 Subject: [PATCH 32/35] Add suggestions Signed-off-by: Jakub Delicat --- .../action/shutdown_hosts_from_file_node.hpp | 2 +- .../action/shutdown_single_host_node.hpp | 2 +- .../panther_manager/plugins/shutdown_host.hpp | 52 +++++++++---------- .../plugins/shutdown_hosts_node.hpp | 32 ++++++------ .../action/shutdown_hosts_from_file_node.cpp | 2 +- .../action/shutdown_single_host_node.cpp | 2 +- .../test_call_set_bool_service_node.cpp | 3 +- .../test/plugins/test_shutdown_host.cpp | 23 ++++++-- .../test/plugins/test_shutdown_hosts_node.cpp | 8 +-- .../test_shutdown_single_host_node.cpp | 4 +- 10 files changed, 73 insertions(+), 57 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 6cfa65629..e33cc5e2e 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -46,7 +46,7 @@ class ShutdownHostsFromFile : public ShutdownHosts } private: - bool update_hosts(std::vector> & hosts) override; + bool UpdateHosts(std::vector> & hosts) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 47b6a2f10..ed0179752 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -49,7 +49,7 @@ class ShutdownSingleHost : public ShutdownHosts } private: - bool update_hosts(std::vector> & hosts) override; + bool UpdateHosts(std::vector> & hosts) override; }; } // namespace panther_manager diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index eb2c6beb9..1887088e9 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -68,17 +68,17 @@ class ShutdownHost ~ShutdownHost() {} - void call() + void Call() { switch (state_) { case ShutdownHostState::IDLE: - if (!is_available()) { + if (!IsAvailable()) { state_ = ShutdownHostState::SKIPPED; break; } try { - request_shutdown(); + RequestShutdown(); } catch (const std::runtime_error & err) { state_ = ShutdownHostState::FAILURE; failure_reason_ = err.what(); @@ -89,7 +89,7 @@ class ShutdownHost case ShutdownHostState::COMMAND_EXECUTED: try { - if (update_response()) { + if (UpdateResponse()) { break; } } catch (const std::runtime_error & err) { @@ -105,11 +105,11 @@ class ShutdownHost break; case ShutdownHostState::PINGING: - if (ping_for_success_ ? !is_available() : true) { + if (ping_for_success_ ? !IsAvailable() : true) { state_ = ShutdownHostState::SUCCESS; break; } - if (timeout_exceeded()) { + if (TimeoutExceeded()) { state_ = ShutdownHostState::FAILURE; failure_reason_ = "Timeout exceeded"; } @@ -120,12 +120,12 @@ class ShutdownHost } } - bool is_available() const + bool IsAvailable() const { return system(("ping -c 1 -w 1 " + ip_ + " > /dev/null").c_str()) == 0; } - void close_connection() + void CloseConnection() { if (ssh_channel_is_closed(channel_)) { return; @@ -138,19 +138,19 @@ class ShutdownHost ssh_free(session_); } - int get_port() const { return port_; } + int GetPort() const { return port_; } - std::string get_ip() const { return ip_; } + std::string GetIp() const { return ip_; } - std::string get_user() const { return user_; } + std::string GetUser() const { return user_; } - std::string get_command() const { return command_; } + std::string GetCommand() const { return command_; } - std::string get_error() const { return failure_reason_; } + std::string GetError() const { return failure_reason_; } - std::string get_response() const { return output_; } + std::string GetResponse() const { return output_; } - ShutdownHostState get_state() const { return state_; } + ShutdownHostState GetState() const { return state_; } bool operator==(const ShutdownHost & other) const { return hash_ == other.hash_; } @@ -178,16 +178,16 @@ class ShutdownHost ssh_session session_; ssh_channel channel_; - void request_shutdown() + void RequestShutdown() { - ssh_execute_command(command_); + SshExecuteCommadn(command_); command_time_ = std::chrono::steady_clock::now(); } - bool update_response() + bool UpdateResponse() { - if (!is_available()) { - close_connection(); + if (!IsAvailable()) { + CloseConnection(); throw std::runtime_error("Lost connection"); } @@ -195,8 +195,8 @@ class ShutdownHost throw std::runtime_error("Channel closed"); } - if (timeout_exceeded()) { - close_connection(); + if (TimeoutExceeded()) { + CloseConnection(); throw std::runtime_error("Timeout exceeded"); } @@ -204,19 +204,19 @@ class ShutdownHost output_.append(buffer_, nbytes_); return true; } - close_connection(); + CloseConnection(); return false; } - bool timeout_exceeded() + bool TimeoutExceeded() { auto now = std::chrono::steady_clock::now(); auto elapsed = std::chrono::duration_cast(now - command_time_); - return elapsed > timeout_ms_ && is_available(); + return elapsed > timeout_ms_ && IsAvailable(); } - void ssh_execute_command(const std::string & command) + void SshExecuteCommadn(const std::string & command) { session_ = ssh_new(); if (session_ == NULL) { diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index b457c83e6..6f1b21dcf 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -47,10 +47,10 @@ class ShutdownHosts : public BT::StatefulActionNode virtual ~ShutdownHosts() = default; // method to be implemented by user - virtual bool update_hosts(std::vector> & hosts) = 0; + virtual bool UpdateHosts(std::vector> & hosts) = 0; // method that can be overridden by user - virtual BT::NodeStatus post_process() + virtual BT::NodeStatus PostProcess() { // return success only when all hosts succeeded if (this->failed_hosts_.size() == 0) { @@ -59,7 +59,7 @@ class ShutdownHosts : public BT::StatefulActionNode return BT::NodeStatus::FAILURE; } - std::vector const get_failed_hosts() { return this->failed_hosts_; } + std::vector const GetFailedHosts() { return this->failed_hosts_; } protected: std::shared_ptr logger_; @@ -74,12 +74,12 @@ class ShutdownHosts : public BT::StatefulActionNode BT::NodeStatus onStart() { - if (!update_hosts(this->hosts_)) { + if (!UpdateHosts(this->hosts_)) { RCLCPP_ERROR_STREAM(*this->logger_, "Cannot update hosts!"); return BT::NodeStatus::FAILURE; } - remove_duplicate_hosts(this->hosts_); + RemoveDuplicatedHosts(this->hosts_); if (this->hosts_.size() <= 0) { RCLCPP_ERROR_STREAM(*this->logger_, "Hosts list is empty! Check configuration!"); return BT::NodeStatus::FAILURE; @@ -92,7 +92,7 @@ class ShutdownHosts : public BT::StatefulActionNode BT::NodeStatus onRunning() { if (this->hosts_to_check_.size() <= 0) { - return post_process(); + return PostProcess(); } if (this->check_host_index_ >= this->hosts_to_check_.size()) { @@ -101,19 +101,19 @@ class ShutdownHosts : public BT::StatefulActionNode auto host_index = this->hosts_to_check_[this->check_host_index_]; auto host = this->hosts_[host_index]; - host->call(); + host->Call(); - switch (host->get_state()) { + switch (host->GetState()) { case ShutdownHostState::RESPONSE_RECEIVED: RCLCPP_INFO_STREAM( - *this->logger_, "Device at: " << host->get_ip() << " response:\n" - << host->get_response()); + *this->logger_, "Device at: " << host->GetIp() << " response:\n" + << host->GetResponse()); check_host_index_++; break; case ShutdownHostState::SUCCESS: - RCLCPP_INFO_STREAM(*this->logger_, "Successfully shutdown device at: " << host->get_ip()); + RCLCPP_INFO_STREAM(*this->logger_, "Successfully shutdown device at: " << host->GetIp()); this->succeeded_hosts_.push_back(host_index); this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); break; @@ -121,7 +121,7 @@ class ShutdownHosts : public BT::StatefulActionNode case ShutdownHostState::FAILURE: RCLCPP_WARN_STREAM( *this->logger_, - "Failed to shutdown device at: " << host->get_ip() << " Error: " << host->get_error()); + "Failed to shutdown device at: " << host->GetIp() << " Error: " << host->GetError()); this->failed_hosts_.push_back(host_index); this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); @@ -129,7 +129,7 @@ class ShutdownHosts : public BT::StatefulActionNode case ShutdownHostState::SKIPPED: RCLCPP_WARN_STREAM( - *this->logger_, "Device at: " << host->get_ip() << " not available, skipping..."); + *this->logger_, "Device at: " << host->GetIp() << " not available, skipping..."); this->skipped_hosts_.push_back(host_index); this->hosts_to_check_.erase(this->hosts_to_check_.begin() + this->check_host_index_); @@ -143,7 +143,7 @@ class ShutdownHosts : public BT::StatefulActionNode return BT::NodeStatus::RUNNING; } - void remove_duplicate_hosts(std::vector> & hosts) + void RemoveDuplicatedHosts(std::vector> & hosts) { std::set seen; @@ -156,7 +156,7 @@ class ShutdownHosts : public BT::StatefulActionNode return false; } else { RCLCPP_WARN_STREAM( - *this->logger_, "Found duplicate host: " << host->get_ip() + *this->logger_, "Found duplicate host: " << host->GetIp() << " Processing only the " "first " "occurrence."); @@ -169,7 +169,7 @@ class ShutdownHosts : public BT::StatefulActionNode void onHalted() { for (auto & host : this->hosts_) { - host->close_connection(); + host->CloseConnection(); } } }; diff --git a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp index 5c2abf9db..4cea7152c 100644 --- a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -28,7 +28,7 @@ namespace panther_manager { -bool ShutdownHostsFromFile::update_hosts(std::vector> & hosts) +bool ShutdownHostsFromFile::UpdateHosts(std::vector> & hosts) { std::string shutdown_hosts_file; if ( diff --git a/panther_manager/plugins/action/shutdown_single_host_node.cpp b/panther_manager/plugins/action/shutdown_single_host_node.cpp index 2b060572b..cda5113ac 100644 --- a/panther_manager/plugins/action/shutdown_single_host_node.cpp +++ b/panther_manager/plugins/action/shutdown_single_host_node.cpp @@ -26,7 +26,7 @@ namespace panther_manager { -bool ShutdownSingleHost::update_hosts(std::vector> & hosts) +bool ShutdownSingleHost::UpdateHosts(std::vector> & hosts) { std::string ip; if (!getInput("ip", ip) || ip == "") { diff --git a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp index 65beb0cfb..b6f8a227d 100644 --- a/panther_manager/test/plugins/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/test_call_set_bool_service_node.cpp @@ -21,6 +21,7 @@ #include #include + class TestCallSetBoolService : public panther_manager::plugin_test_utils::PluginTestUtils { public: @@ -142,7 +143,7 @@ TEST_F(TestCallSetBoolService, WrongServiceValueDefined) CreateService( "set_bool", [&](const SetBool::Request::SharedPtr request, SetBool::Response::SharedPtr response) { - ServiceCallback(request, response, false, true); + ServiceCallback(request, response, true, true); }); RegisterNodeWithParams("CallSetBoolService"); diff --git a/panther_manager/test/plugins/test_shutdown_host.cpp b/panther_manager/test/plugins/test_shutdown_host.cpp index dcb1ea26c..8b0cfe588 100644 --- a/panther_manager/test/plugins/test_shutdown_host.cpp +++ b/panther_manager/test/plugins/test_shutdown_host.cpp @@ -41,16 +41,16 @@ void TestShutdownHost::Create( ip, user, port, command, timeout, ping_for_success); } -bool TestShutdownHost::IsAvailable() { return shutdown_host->is_available(); } +bool TestShutdownHost::IsAvailable() { return shutdown_host->IsAvailable(); } -void TestShutdownHost::Call() { shutdown_host->call(); } +void TestShutdownHost::Call() { shutdown_host->Call(); } panther_manager::ShutdownHostState TestShutdownHost::GetState() const { - return shutdown_host->get_state(); + return shutdown_host->GetState(); } -std::string TestShutdownHost::GetResponse() const { return shutdown_host->get_response(); } +std::string TestShutdownHost::GetResponse() const { return shutdown_host->GetResponse(); } TEST_F(TestShutdownHost, GoodCheckIsAvailable) { @@ -98,6 +98,21 @@ TEST_F(TestShutdownHost, WrongHostPing) EXPECT_EQ(GetState(), panther_manager::ShutdownHostState::SKIPPED); } +TEST_F(TestShutdownHost, CheckTimeout) +{ + Create("127.0.0.1", "husarion", 22, "sleep 0.2", 0.1, false); + + ASSERT_TRUE(IsAvailable()); + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::IDLE); + Call(); + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::COMMAND_EXECUTED); + // Wait for response + while (GetState() == panther_manager::ShutdownHostState::COMMAND_EXECUTED) { + Call(); + } + ASSERT_EQ(GetState(), panther_manager::ShutdownHostState::FAILURE); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index ff1f072cc..fe7f6aa00 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -19,10 +19,10 @@ #include #include -class ShutdownHostsNodeDuplicated : public panther_manager::ShutdownHosts +class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts { public: - ShutdownHostsNodeDuplicated(const std::string & name, const BT::NodeConfig & conf) + ShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) : panther_manager::ShutdownHosts(name, conf) { } @@ -30,11 +30,11 @@ class ShutdownHostsNodeDuplicated : public panther_manager::ShutdownHosts static BT::PortsList providedPorts() { return {}; } private: - virtual bool update_hosts( + virtual bool UpdateHosts( std::vector> & hosts) override final; }; -bool ShutdownHostsNodeDuplicated::update_hosts( +bool ShutdownHostsNodeWrapper::UpdateHosts( std::vector> & hosts) { hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 58f141f0a..985bf295e 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -113,7 +113,7 @@ TEST_F(TestShutdownSingleHost, WrongUser) {"ip", "localhost"}, {"ping_for_success", "false"}, {"port", "22"}, - {"timeout", "5.0"}, + {"timeout", "0.2"}, {"username", "wrong_user"}, }; RegisterNodeWithoutParams("ShutdownSingleHost"); @@ -122,7 +122,7 @@ TEST_F(TestShutdownSingleHost, WrongUser) auto & tree = GetTree(); CreateTree("ShutdownSingleHost", service); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } From 4af614ed5e42bee33d2f77ac44a8c8cc6c3a5320 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 25 Mar 2024 12:37:44 +0000 Subject: [PATCH 33/35] Added shutdonw hosts node Signed-off-by: Jakub Delicat --- .../plugins/shutdown_hosts_node.hpp | 2 - .../test/plugins/test_shutdown_hosts_node.cpp | 91 +++++++++++++++++-- 2 files changed, 85 insertions(+), 8 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp index 6f1b21dcf..224b64c12 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -63,8 +63,6 @@ class ShutdownHosts : public BT::StatefulActionNode protected: std::shared_ptr logger_; - -private: std::size_t check_host_index_ = 0; std::vector> hosts_; std::vector hosts_to_check_; diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index fe7f6aa00..933995afc 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -19,6 +19,8 @@ #include #include +typedef panther_manager::plugin_test_utils::PluginTestUtils ShutdownHostsNodeBehaviorTreeTest; + class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts { public: @@ -26,15 +28,33 @@ class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts : panther_manager::ShutdownHosts(name, conf) { } + void RemoveDuplicatedHosts(std::vector> & hosts); + std::vector> & GetHosts(); +}; - static BT::PortsList providedPorts() { return {}; } +void ShutdownHostsNodeWrapper::RemoveDuplicatedHosts( + std::vector> & hosts) +{ + panther_manager::ShutdownHosts::RemoveDuplicatedHosts(hosts); +} -private: - virtual bool UpdateHosts( - std::vector> & hosts) override final; +std::vector> & ShutdownHostsNodeWrapper::GetHosts() +{ + return hosts_; +} + +class DuplicatedHostsShutdownHostsNodeWrapper : public ShutdownHostsNodeWrapper +{ +public: + DuplicatedHostsShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHostsNodeWrapper(name, conf) + { + } + bool UpdateHosts(std::vector> & hosts); + static BT::PortsList providedPorts() { return {}; } }; -bool ShutdownHostsNodeWrapper::UpdateHosts( +bool DuplicatedHostsShutdownHostsNodeWrapper::UpdateHosts( std::vector> & hosts) { hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); @@ -43,7 +63,66 @@ bool ShutdownHostsNodeWrapper::UpdateHosts( return true; } -// TODO: Create TESTs completely +class FailedUpdateHostsShutdownHostsNodeWrapper : public ShutdownHostsNodeWrapper +{ +public: + FailedUpdateHostsShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHostsNodeWrapper(name, conf) + { + } + bool UpdateHosts(std::vector> & hosts); + static BT::PortsList providedPorts() { return {}; } +}; + +bool FailedUpdateHostsShutdownHostsNodeWrapper::UpdateHosts( + std::vector> & hosts) +{ + hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); + hosts.emplace_back(std::make_shared("localhost", "husarion")); + return false; +} + +class ShutdownHostsNodeTest : public testing::Test +{ +public: + void CreateDuplicationWrapper(); + void CreateFailedUpdatedHostWrapper(); + +protected: + std::unique_ptr wrapper; +}; + +void ShutdownHostsNodeTest::CreateDuplicationWrapper() +{ + BT::NodeConfig conf; + wrapper = std::make_unique("Duplicated hosts", conf); +} + +void ShutdownHostsNodeTest::CreateFailedUpdatedHostWrapper() +{ + BT::NodeConfig conf; + wrapper = std::make_unique("Failed hosts", conf); +} + +TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) +{ + CreateDuplicationWrapper(); + std::vector> hosts; + ASSERT_TRUE(wrapper->UpdateHosts(hosts)); + ASSERT_EQ(hosts.size(), 3); + wrapper->RemoveDuplicatedHosts(hosts); + ASSERT_EQ(hosts.size(), 2); +} + +TEST_F(ShutdownHostsNodeBehaviorTreeTest, FailedBehaviorTreeWhenUpdateHostReturnsFalse) +{ + RegisterNodeWithoutParams("ShutdownHosts"); + CreateTree("ShutdownHosts", {}); + auto & tree = GetTree(); + + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} int main(int argc, char ** argv) { From e4f95222506dc595b1c63a3562a6f0dc104b713c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 25 Mar 2024 21:18:25 +0000 Subject: [PATCH 34/35] Fixed typo | cleaned up the test_shutdown_hosts_node Signed-off-by: Jakub Delicat --- .../panther_manager/plugins/shutdown_host.hpp | 4 +- .../test/plugins/test_shutdown_hosts_node.cpp | 123 +++++++++++------- 2 files changed, 77 insertions(+), 50 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp index 1887088e9..712b22c0c 100644 --- a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -180,7 +180,7 @@ class ShutdownHost void RequestShutdown() { - SshExecuteCommadn(command_); + SshExecuteCommand(command_); command_time_ = std::chrono::steady_clock::now(); } @@ -216,7 +216,7 @@ class ShutdownHost return elapsed > timeout_ms_ && IsAvailable(); } - void SshExecuteCommadn(const std::string & command) + void SshExecuteCommand(const std::string & command) { session_ = ssh_new(); if (session_ == NULL) { diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index 933995afc..962a0fb8d 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -17,9 +17,6 @@ #include #include -#include - -typedef panther_manager::plugin_test_utils::PluginTestUtils ShutdownHostsNodeBehaviorTreeTest; class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts { @@ -30,6 +27,24 @@ class ShutdownHostsNodeWrapper : public panther_manager::ShutdownHosts } void RemoveDuplicatedHosts(std::vector> & hosts); std::vector> & GetHosts(); + BT::NodeStatus onRunning(); + BT::NodeStatus onStart(); + + virtual bool UpdateHosts( + std::vector> & hosts) override final; + void SetHostsAndSuccess( + std::vector> hosts, const bool returned_status); + + static BT::PortsList providedPorts() + { + return { + + }; + } + +private: + std::vector> hosts_to_set; + bool update_hosts_success_ = true; }; void ShutdownHostsNodeWrapper::RemoveDuplicatedHosts( @@ -43,70 +58,58 @@ std::vector> & ShutdownHostsNodeW return hosts_; } -class DuplicatedHostsShutdownHostsNodeWrapper : public ShutdownHostsNodeWrapper +BT::NodeStatus ShutdownHostsNodeWrapper::onRunning() { -public: - DuplicatedHostsShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) - : ShutdownHostsNodeWrapper(name, conf) - { - } - bool UpdateHosts(std::vector> & hosts); - static BT::PortsList providedPorts() { return {}; } -}; + return panther_manager::ShutdownHosts::onRunning(); +} -bool DuplicatedHostsShutdownHostsNodeWrapper::UpdateHosts( - std::vector> & hosts) +BT::NodeStatus ShutdownHostsNodeWrapper::onStart() { - hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); - hosts.emplace_back(std::make_shared("localhost", "husarion")); - hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); - return true; + return panther_manager::ShutdownHosts::onStart(); } -class FailedUpdateHostsShutdownHostsNodeWrapper : public ShutdownHostsNodeWrapper +bool ShutdownHostsNodeWrapper::UpdateHosts( + std::vector> & hosts) { -public: - FailedUpdateHostsShutdownHostsNodeWrapper(const std::string & name, const BT::NodeConfig & conf) - : ShutdownHostsNodeWrapper(name, conf) - { - } - bool UpdateHosts(std::vector> & hosts); - static BT::PortsList providedPorts() { return {}; } -}; + hosts = hosts_to_set; + return update_hosts_success_; +} -bool FailedUpdateHostsShutdownHostsNodeWrapper::UpdateHosts( - std::vector> & hosts) +void ShutdownHostsNodeWrapper::SetHostsAndSuccess( + std::vector> hosts, const bool returned_status) { - hosts.emplace_back(std::make_shared("127.0.0.1", "husarion")); - hosts.emplace_back(std::make_shared("localhost", "husarion")); - return false; + hosts_to_set = hosts; + update_hosts_success_ = returned_status; } class ShutdownHostsNodeTest : public testing::Test { public: - void CreateDuplicationWrapper(); - void CreateFailedUpdatedHostWrapper(); + void CreateWrapper( + std::vector> hosts, const bool success); protected: std::unique_ptr wrapper; }; -void ShutdownHostsNodeTest::CreateDuplicationWrapper() +void ShutdownHostsNodeTest::CreateWrapper( + std::vector> hosts, const bool success) { BT::NodeConfig conf; - wrapper = std::make_unique("Duplicated hosts", conf); -} - -void ShutdownHostsNodeTest::CreateFailedUpdatedHostWrapper() -{ - BT::NodeConfig conf; - wrapper = std::make_unique("Failed hosts", conf); + wrapper = std::make_unique("Duplicated hosts", conf); + wrapper->SetHostsAndSuccess(hosts, success); } TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) { - CreateDuplicationWrapper(); + CreateWrapper( + {std::make_shared( + "127.0.0.1", "husarion", 22, "echo HelloWorld", 100, true), + std::make_shared( + "localhost", "husarion", 22, "echo HelloWorld", 100, true), + std::make_shared( + "localhost", "husarion", 22, "echo HelloWorld", 100, true)}, + true); std::vector> hosts; ASSERT_TRUE(wrapper->UpdateHosts(hosts)); ASSERT_EQ(hosts.size(), 3); @@ -114,16 +117,40 @@ TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) ASSERT_EQ(hosts.size(), 2); } -TEST_F(ShutdownHostsNodeBehaviorTreeTest, FailedBehaviorTreeWhenUpdateHostReturnsFalse) +TEST_F(ShutdownHostsNodeTest, FailedWhenUpdateHostReturnsFalse) { - RegisterNodeWithoutParams("ShutdownHosts"); - CreateTree("ShutdownHosts", {}); - auto & tree = GetTree(); + CreateWrapper( + {std::make_shared( + "127.0.0.1", "husarion", 22, "echo HelloWorld", 100, true)}, + false); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = wrapper->onStart(); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } +TEST_F(ShutdownHostsNodeTest, FailedWhenHostsAreEmpty) +{ + CreateWrapper({}, true); + + auto status = wrapper->onStart(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +// FIXME: This test pass but the host should be added to the failed hosts. +// TEST_F(ShutdownHostsNodeTest, CheckFailedHosts) +// { +// CreateWrapper({ std::make_shared("127.0.0.1", "husarion", 22, +// "wrong_command", 100, false) }, +// true); +// auto status = wrapper->onStart(); +// EXPECT_EQ(status, BT::NodeStatus::RUNNING); +// while(wrapper->GetFailedHosts().size() == 0 && status != BT::NodeStatus::SUCCESS){ +// status = wrapper->onRunning(); +// } +// status = wrapper->onRunning(); +// EXPECT_EQ(wrapper->GetFailedHosts().size(), 0); +// } + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 115ddb7a8988119a8bd9ca9455ae9c1e0eb5084b Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 26 Mar 2024 10:39:32 +0000 Subject: [PATCH 35/35] fix tests --- .../test/plugins/test_shutdown_hosts_node.cpp | 36 +++++++++---------- .../test_shutdown_single_host_node.cpp | 21 +---------- 2 files changed, 19 insertions(+), 38 deletions(-) diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index 962a0fb8d..0d630aaa5 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -104,11 +104,11 @@ TEST_F(ShutdownHostsNodeTest, GoodRemovingDuplicatedHosts) { CreateWrapper( {std::make_shared( - "127.0.0.1", "husarion", 22, "echo HelloWorld", 100, true), + "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true), std::make_shared( - "localhost", "husarion", 22, "echo HelloWorld", 100, true), + "localhost", "husarion", 22, "echo HelloWorld", 1.0, true), std::make_shared( - "localhost", "husarion", 22, "echo HelloWorld", 100, true)}, + "localhost", "husarion", 22, "echo HelloWorld", 1.0, true)}, true); std::vector> hosts; ASSERT_TRUE(wrapper->UpdateHosts(hosts)); @@ -121,7 +121,7 @@ TEST_F(ShutdownHostsNodeTest, FailedWhenUpdateHostReturnsFalse) { CreateWrapper( {std::make_shared( - "127.0.0.1", "husarion", 22, "echo HelloWorld", 100, true)}, + "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)}, false); auto status = wrapper->onStart(); @@ -136,20 +136,20 @@ TEST_F(ShutdownHostsNodeTest, FailedWhenHostsAreEmpty) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -// FIXME: This test pass but the host should be added to the failed hosts. -// TEST_F(ShutdownHostsNodeTest, CheckFailedHosts) -// { -// CreateWrapper({ std::make_shared("127.0.0.1", "husarion", 22, -// "wrong_command", 100, false) }, -// true); -// auto status = wrapper->onStart(); -// EXPECT_EQ(status, BT::NodeStatus::RUNNING); -// while(wrapper->GetFailedHosts().size() == 0 && status != BT::NodeStatus::SUCCESS){ -// status = wrapper->onRunning(); -// } -// status = wrapper->onRunning(); -// EXPECT_EQ(wrapper->GetFailedHosts().size(), 0); -// } +TEST_F(ShutdownHostsNodeTest, CheckFailedHosts) +{ + CreateWrapper( + {std::make_shared( + "127.0.0.1", "husarion", 22, "echo HelloWorld", 1.0, true)}, + true); + auto status = wrapper->onStart(); + EXPECT_EQ(status, BT::NodeStatus::RUNNING); + while (status == BT::NodeStatus::RUNNING) { + status = wrapper->onRunning(); + } + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + EXPECT_EQ(wrapper->GetFailedHosts().size(), 1); +} int main(int argc, char ** argv) { diff --git a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp index 985bf295e..d5594d00b 100644 --- a/panther_manager/test/plugins/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_single_host_node.cpp @@ -87,25 +87,6 @@ TEST_F(TestShutdownSingleHost, Timeout) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST_F(TestShutdownSingleHost, WrongCommand) -{ - std::map service = { - {"command", "wrong_command"}, - {"ip", "localhost"}, - {"ping_for_success", "false"}, - {"port", "22"}, - {"timeout", "0.2"}, - {"username", "husarion"}, - }; - RegisterNodeWithoutParams("ShutdownSingleHost"); - - CreateTree("ShutdownSingleHost", service); - auto & tree = GetTree(); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - TEST_F(TestShutdownSingleHost, WrongUser) { std::map service = { @@ -122,7 +103,7 @@ TEST_F(TestShutdownSingleHost, WrongUser) auto & tree = GetTree(); CreateTree("ShutdownSingleHost", service); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(300)); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); }