From f4071af5dea80639ede08ebac79bff6525f6ca6f Mon Sep 17 00:00:00 2001 From: Crasun Jans Date: Thu, 5 Dec 2024 14:10:23 -0600 Subject: [PATCH] add bt integration with ros2 tutorials --- bt_tutorials_ros2/.README.md.swp | Bin 0 -> 12288 bytes bt_tutorials_ros2/CMakeLists.txt | 75 ++++++++++++ bt_tutorials_ros2/README.md | 50 ++++++++ .../bt_tutorials_ros2_action_client.hpp | 115 ++++++++++++++++++ .../bt_tutorials_ros2_service_client.hpp | 100 +++++++++++++++ bt_tutorials_ros2/package.xml | 26 ++++ .../bt_tutorials_ros2_action_client.cpp | 72 +++++++++++ .../bt_tutorials_ros2_service_client.cpp | 51 ++++++++ .../bt_tutorials_ros2_action_client_node.cpp | 54 ++++++++ .../bt_tutorials_ros2_service_client_node.cpp | 54 ++++++++ 10 files changed, 597 insertions(+) create mode 100644 bt_tutorials_ros2/.README.md.swp create mode 100644 bt_tutorials_ros2/CMakeLists.txt create mode 100644 bt_tutorials_ros2/README.md create mode 100644 bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp create mode 100644 bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_service_client.hpp create mode 100644 bt_tutorials_ros2/package.xml create mode 100644 bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_action_client.cpp create mode 100644 bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_service_client.cpp create mode 100644 bt_tutorials_ros2/src/bt_tutorials_ros2_action_client_node.cpp create mode 100644 bt_tutorials_ros2/src/bt_tutorials_ros2_service_client_node.cpp diff --git a/bt_tutorials_ros2/.README.md.swp b/bt_tutorials_ros2/.README.md.swp new file mode 100644 index 0000000000000000000000000000000000000000..12083507987806416e092f7d4bb8073351baba71 GIT binary patch literal 12288 zcmeHN&yN&E6fTHA5M?!Z@?r`%G3+qi4XY94HVzO`$ZmF*B?@Myr@LmRq^GN?uAX6| zQU3tBsnM9IiBW?56(029LC+q9KR^!Ny!usDPtWWQyC5+kYEANGcDrBw`riBAtIfX7 z;q}vJ>C65Rg3nWge70w$_ss5#loe7;p?Y1{?#90mp!2z%k$$a11yG90QI4$H2dkfe`;l z|LPG!K6n(H$N&GwfB)Zpl91nlTfj}=yE#Ix1J43i_Yv|T@GkHU@H+53@aGeR`~v(8 zd=GpId;~;53s?hIfuq1nz$cFr@-c7~I1j7>m-iBq0dv4!;HSq3`4RX5xCU(OA*2A_ z1lECbz#rKAZQurQ4fq!L9MEUF4txWg#~Igw6<|Lw2i$y^kl%n`fvgxKI9+El=OU#q`iH5I zw9k8eCapP|fas#OS!20kF@=?RL|-iiorGDV=AlDJj%uS7cDdjZW?f9vDR>g|w1*uq znuXD&um^iGKR-4buvFzZT)W*Sy0wVPBBe78N3%~_Ig>*ku~w9DOwme3%*-T9X~fKG zr2*C7Oi~a~WOceB3L1r}Sw`<^y4%T^Voqx3qy#U&F}^a?#{0n`FDrj|8nTq zD&2Ws3s-Gl|F@Vx*IamsE{7?ELJ!(bI;vag77(iQv{9(OkPj|MYqdrkuZ_fMXl18} zt<|cS?H}K=-6+dixf~+2OGOy9oJ{jVTD`P`FpgVlBw8HHv@0=8^aE+8(6Ue8C`H5v zS;Dk}>ogLBj3=y4!!)kMRSxZnBoU)=77*PokI+h3=8$n`A-#Zn!`?dBS1QYo2SF^N z+($<6h3o|^4f-b61Z(3)MD!{4c$zZlHBAQa&hU;5C1-hXUUG%iyoR@GcIY*;%v-hj z!fT{)Il}L&L9*b}Q^-^7N~q5=n;*&}APay|6AKehlv7f>fh~OBOAZSog_OH}3bXI; zlEG@VQ@kV6FpBu**eFNaWVR(VPIs#uX>(&L6b*+VPeNq$2@TeD#`d4`8-ke00gHL4 zSmmlP^%{l@E}6Y!+*7Tb6t4$+7ty72D&KBa zxxV2vHtWm^%w}GsP9}AH;L20Ex_xoS%-WS=Fr%==aZ1YV^=cb8-`{q&jpe&;g_sC0 zgNJ5P40+6Q+AVaDBV`x58nC#oOF*hqNp%b=Y|0tOy<$CS!+u-=FwG1T7pCuL`fgJ# zV#ZF&#b`A0J7wp((w*xu8wLv~N8{2{K$cLXpbD4RIvt3ZcQ$-8 // Action definition for Fibonacci action. +#include // Base class for action nodes in BehaviorTreeCpp. +#include // BT wrapper for ROS2 actions. +#include // For handling string types. + +namespace bt_tutorials_ros2_action_client +{ +// Alias for the Fibonacci action type. +using Fibonacci = action_tutorials_interfaces::action::Fibonacci; + +/** + * @brief Class representing a custom action node for the Fibonacci action in a BehaviorTree. + * + * This class derives from the `RosActionNode` provided by the `behaviortree_ros2` library. + * It implements all necessary callbacks and methods to interact with an action server + * to perform the Fibonacci action. + */ +class FibonacciActionNode : public BT::RosActionNode +{ + public: + // Type alias for the GoalHandle specific to the Fibonacci action. + using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + + /** + * @brief Constructor for the FibonacciActionNode. + * + * Initializes the action node with the given name, configuration, and ROS node parameters. + * This allows the action node to interact with the ROS 2 system and communicate with the action server. + * + * @param name The name of the node in the behavior tree. + * @param conf The configuration parameters for the behavior tree node. + * @param params ROS node parameters used to configure the action node. + */ + FibonacciActionNode(const std::string &name, const BT::NodeConfig &conf, const BT::RosNodeParams ¶ms); + + /** + * @brief Provides the ports required by this action node. + * + * This static function defines the input and output ports for the node, merging the + * base class ports with any additional ones specific to this derived class. + * + * @return A list of ports this action node provides. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Sends a request to the action server when the tree node is ticked. + * + * This function is called when the behavior tree ticks the node. It sends the goal to the + * action server. + * + * @param goal The goal to be sent to the action server. + * @return True if the goal was successfully set, otherwise false. + */ + bool setGoal(RosActionNode::Goal &goal) override; + + /** + * @brief Callback executed when a result is received from the action server. + * + * This function is invoked when the action server sends a result. Based on the result, + * it will return either `SUCCESS` or `FAILURE` to indicate the outcome of the action. + * + * @param wr The wrapped result received from the action server. + * @return The status of the node after processing the result. + */ + BT::NodeStatus onResultReceived(const WrappedResult &wr) override; + + /** + * @brief Callback invoked when the action client encounters a communication failure. + * + * This callback handles failure cases where the communication between the action client + * and the server fails. The node status is updated to either `SUCCESS` or `FAILURE` based on + * the error received. + * + * @param error The error code indicating the failure reason. + * @return The status of the node after handling the failure. + */ + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override; + + /** + * @brief Callback for receiving feedback from the action server. + * + * This function is invoked when feedback is received from the action server. The feedback + * can be used to determine the current progress of the action. If necessary, the action can + * be aborted based on feedback, or the node can return `SUCCESS` or `FAILURE` if the action completes. + * + * @param feedback The feedback received from the action server. + * @return The status of the node after processing the feedback. + */ + BT::NodeStatus onFeedback(const std::shared_ptr feedback); + + private: + std::shared_ptr shared_node_; // shared pointer to the ROS node +}; + +} // namespace bt_tutorials_ros2_action_client + +#endif // BT_TUTORIALS_ROS2_ACTION_CLIENT_HPP diff --git a/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_service_client.hpp b/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_service_client.hpp new file mode 100644 index 0000000..4c66171 --- /dev/null +++ b/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_service_client.hpp @@ -0,0 +1,100 @@ +/** + * @file bt_tutorials_ros2_service_client.hpp + * @brief Header file for the AddTwoIntsNode, a custom BehaviorTree node for interacting with the official ROS 2 + * "/add_two_ints" service. + * + * @author Janak Panthi (Crasun Jans) + * + * @details Implements the AddTwoInts service client tutorial from the "Integration with ROS2" section on the + * official BehaviorTree.CPP website. This node interacts with a ROS 2 service server to send two integers as a + * request and processes the resulting sum. + * + * @license MIT + */ + +#ifndef BT_TUTORIALS_ROS2_SERVICE_CLIENT_HPP +#define BT_TUTORIALS_ROS2_SERVICE_CLIENT_HPP + +#include // Base class for action nodes in BehaviorTreeCpp. +#include // BT wrapper for ROS2 service nodes. +#include // Service definition for AddTwoInts service. +#include // For handling string types. + +namespace bt_tutorials_ros2_service_client +{ +// Alias for the AddTwoInts service type. +using AddTwoInts = example_interfaces::srv::AddTwoInts; + +/** + * @brief Class representing a custom service node for the AddTwoInts service in a BehaviorTree. + * + * This class derives from the `RosServiceNode` provided by the `behaviortree_ros2` library. + * It implements all necessary callbacks and methods to interact with a service server + * to perform the AddTwoInts service call. + */ +class AddTwoIntsNode : public BT::RosServiceNode +{ + public: + /** + * @brief Constructor for the AddTwoIntsNode. + * + * Initializes the service node with the given name, configuration, and ROS node parameters. + * This allows the service node to communicate with the ROS 2 system and the service server. + * + * @param name The name of the node in the behavior tree. + * @param conf The configuration parameters for the behavior tree node. + * @param params ROS node parameters used to configure the service node. + */ + AddTwoIntsNode(const std::string &name, const BT::NodeConfig &conf, const BT::RosNodeParams ¶ms); + + /** + * @brief Provides the ports required by this service node. + * + * This static function defines the input and output ports for the node, merging the + * base class ports with any additional ones specific to this derived class. + * + * @return A list of ports this service node provides. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Sends a request to the service server when the tree node is ticked. + * + * This function is called when the behavior tree ticks the node. It sends the request to the + * service server. + * + * @param request The request to be sent to the service server. + * @return True if the request was successfully set, otherwise false. + */ + bool setRequest(Request::SharedPtr &request) override; + + /** + * @brief Callback executed when a response is received from the service server. + * + * This function is invoked when the service server sends a response. Based on the response, + * it will return either `SUCCESS` or `FAILURE` to indicate the outcome of the service call. + * + * @param response The response received from the service server. + * @return The status of the node after processing the response. + */ + BT::NodeStatus onResponseReceived(const Response::SharedPtr &response) override; + + /** + * @brief Callback invoked when the service client encounters a communication failure. + * + * This callback handles failure cases where the communication between the service client + * and the server fails. The node status is updated to either `SUCCESS` or `FAILURE` based on + * the error received. + * + * @param error The error code indicating the failure reason. + * @return The status of the node after handling the failure. + */ + BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override; + + private: + std::shared_ptr shared_node_; // Shared pointer to the ROS node for logging and other tasks. +}; + +} // namespace bt_tutorials_ros2_service_client + +#endif // BT_TUTORIALS_ROS2_SERVICE_CLIENT_HPP diff --git a/bt_tutorials_ros2/package.xml b/bt_tutorials_ros2/package.xml new file mode 100644 index 0000000..bd95fc6 --- /dev/null +++ b/bt_tutorials_ros2/package.xml @@ -0,0 +1,26 @@ + + + + bt_tutorials_ros2 + 0.0.0 + Implementation of the Integration with ROS2 Tutorials from the behaviortree.dev Website + Janak Panthi + MIT + Janak Panthi + + ament_cmake + + rclcpp + rclcpp_action + action_tutorials_interfaces + example_interfaces + behaviortree_cpp + behaviortree_ros2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_action_client.cpp b/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_action_client.cpp new file mode 100644 index 0000000..e01539e --- /dev/null +++ b/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_action_client.cpp @@ -0,0 +1,72 @@ +#include "bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp" + +namespace bt_tutorials_ros2_action_client +{ + +FibonacciActionNode::FibonacciActionNode(const std::string &name, const BT::NodeConfig &conf, + const BT::RosNodeParams ¶ms) + : BT::RosActionNode(name, conf, params) +{ + // Convert weak_ptr (from ROS params) to shared_ptr for usage in ROS-related tasks like logging + auto shared_node = params.nh.lock(); + if (!shared_node) + { + throw std::runtime_error("FibonacciActionNode: Failed to lock node from params.nh"); + } + shared_node_ = shared_node; +} +BT::PortsList FibonacciActionNode::providedPorts() +{ + // Return the basic ports provided by the base class, along with a new input port "order" + return RosActionNode::providedBasicPorts({BT::InputPort("order")}); +} + +bool FibonacciActionNode::setGoal(RosActionNode::Goal &goal) +{ + + // Retrieve the value from the "order" port and set it in the goal object + getInput("order", goal.order); + return true; // Return true after successfully setting the goal +} + +BT::NodeStatus FibonacciActionNode::onResultReceived(const WrappedResult &wr) +{ + std::stringstream ss; + ss << "Result received: "; + + // Iterate over the result sequence and log each number received from the server + for (auto number : wr.result->sequence) + { + ss << number << " "; + } + + // Log the result to ROS + RCLCPP_INFO(shared_node_->get_logger(), ss.str().c_str()); + return BT::NodeStatus::SUCCESS; // Return success after processing the result +} + +BT::NodeStatus FibonacciActionNode::onFailure(BT::ActionNodeErrorCode error) +{ + // Log the error code if the communication fails and return failure + RCLCPP_ERROR(shared_node_->get_logger(), "Error: %d", error); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus FibonacciActionNode::onFeedback(const std::shared_ptr feedback) +{ + std::stringstream ss; + ss << "Next number in sequence received: "; + + // Log the partial sequence received as feedback from the action server + + for (auto number : feedback->partial_sequence) + { + ss << number << " "; + } + + // Log the feedback to ROS and return RUNNING + RCLCPP_INFO(shared_node_->get_logger(), ss.str().c_str()); + return BT::NodeStatus::RUNNING; +} + +} // namespace bt_tutorials_ros2_action_client diff --git a/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_service_client.cpp b/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_service_client.cpp new file mode 100644 index 0000000..433404f --- /dev/null +++ b/bt_tutorials_ros2/src/bt_tutorials_ros2/bt_tutorials_ros2_service_client.cpp @@ -0,0 +1,51 @@ +#include "bt_tutorials_ros2/bt_tutorials_ros2_service_client.hpp" + +namespace bt_tutorials_ros2_service_client +{ + +AddTwoIntsNode::AddTwoIntsNode(const std::string &name, const BT::NodeConfig &conf, const BT::RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) +{ + // Convert weak_ptr (from ROS params) to shared_ptr for usage in ROS-related tasks like logging + auto shared_node = params.nh.lock(); + if (!shared_node) + { + throw std::runtime_error("AddTwoIntsNode: Failed to lock node from params.nh"); + } + shared_node_ = shared_node; +} + +BT::PortsList AddTwoIntsNode::providedPorts() +{ + // Return the basic ports provided by the base class, along with new input ports "A" and "B" + return RosServiceNode::providedBasicPorts({BT::InputPort("A"), BT::InputPort("B")}); +} + +bool AddTwoIntsNode::setRequest(Request::SharedPtr &request) +{ + // Get numbers to add from the input ports + getInput("A", request->a); + getInput("B", request->b); + + // Return true if we were able to set the request correctly + return true; +} + +BT::NodeStatus AddTwoIntsNode::onResponseReceived(const Response::SharedPtr &response) +{ + // Log the sum received from the service server + RCLCPP_INFO(shared_node_->get_logger(), "Sum: %ld", response->sum); + + // Return success after processing the response + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus AddTwoIntsNode::onFailure(BT::ServiceNodeErrorCode error) +{ + // Log the error code if communication with the service server fails and return failure + RCLCPP_ERROR(shared_node_->get_logger(), "Error: %d", error); + + return BT::NodeStatus::FAILURE; +} + +} // namespace bt_tutorials_ros2_service_client diff --git a/bt_tutorials_ros2/src/bt_tutorials_ros2_action_client_node.cpp b/bt_tutorials_ros2/src/bt_tutorials_ros2_action_client_node.cpp new file mode 100644 index 0000000..70bc83c --- /dev/null +++ b/bt_tutorials_ros2/src/bt_tutorials_ros2_action_client_node.cpp @@ -0,0 +1,54 @@ +#include "bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp" +#include +#include + +using namespace bt_tutorials_ros2_action_client; + +// Define the XML representation of the behavior tree +static const char *xml_text = R"( + + + +