diff --git a/bt_tutorials_ros2/.README.md.swp b/bt_tutorials_ros2/.README.md.swp new file mode 100644 index 0000000..1208350 Binary files /dev/null and b/bt_tutorials_ros2/.README.md.swp differ diff --git a/bt_tutorials_ros2/CMakeLists.txt b/bt_tutorials_ros2/CMakeLists.txt new file mode 100644 index 0000000..654de7c --- /dev/null +++ b/bt_tutorials_ros2/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.8) +project(bt_tutorials_ros2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(action_tutorials_interfaces REQUIRED) +find_package(example_interfaces REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) + +# include directories +include_directories( + include +) + +# add libraries +add_library(${PROJECT_NAME}_action_client src/${PROJECT_NAME}/${PROJECT_NAME}_action_client.cpp) +add_library(${PROJECT_NAME}_service_client src/${PROJECT_NAME}/${PROJECT_NAME}_service_client.cpp) + +# link libraries against ros dependencies +ament_target_dependencies(${PROJECT_NAME}_action_client rclcpp behaviortree_cpp behaviortree_ros2 rclcpp_action action_tutorials_interfaces) +ament_target_dependencies(${PROJECT_NAME}_service_client rclcpp behaviortree_cpp behaviortree_ros2 example_interfaces) + +# add executables +add_executable(${PROJECT_NAME}_action_client_node src/${PROJECT_NAME}_action_client_node.cpp) +add_executable(${PROJECT_NAME}_service_client_node src/${PROJECT_NAME}_service_client_node.cpp) + +# link executables against libraries +target_link_libraries(${PROJECT_NAME}_action_client_node ${PROJECT_NAME}_action_client) +target_link_libraries(${PROJECT_NAME}_service_client_node ${PROJECT_NAME}_service_client) + +# link executables against ros dependencies +ament_target_dependencies(${PROJECT_NAME}_action_client_node rclcpp behaviortree_cpp) +ament_target_dependencies(${PROJECT_NAME}_service_client_node rclcpp behaviortree_cpp) + +# install include directories +install( + DIRECTORY include + DESTINATION include +) + +# install libraries +install( + TARGETS ${PROJECT_NAME}_action_client ${PROJECT_NAME}_service_client + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# install executables +install( + TARGETS ${PROJECT_NAME}_action_client_node ${PROJECT_NAME}_service_client_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/bt_tutorials_ros2/README.md b/bt_tutorials_ros2/README.md new file mode 100644 index 0000000..9ff6f93 --- /dev/null +++ b/bt_tutorials_ros2/README.md @@ -0,0 +1,50 @@ +# bt_tutorials_ros2 + +## Overview + +This package provides fully documented, runnable examples of the ROS 2 integration tutorials from the official [BehaviorTree.CPP documentation](https://www.behaviortree.dev/), allowing users to test, modify, and extend these examples in their own projects. Specifically, the following two nodes are created: + +- **Fibonacci Action Client**: A BehaviorTree client node interacting with the `\fibonacci` action server from the official ROS2 examples. +- **AddTwoInts Service Client**: A BehaviorTree client node interacting with the `\add_two_ints` service server from the official ROS2 examples. + +## Requirements + +- **C++17**: Required for building the package. +- **ROS 2**: Tested on ROS 2 Humble. +- **BehaviorTree.CPP**: Version 4.0 or higher. +- **BehaviorTree.ROS**, i.e. this repo. + +## Compiling +Compile using `colcon`, for instance as: +``` +colcon build --packages-select bt_tutorials_ros2 +``` +## Running + +### Fibonacci Action Client + +Ensure the `/fibonacci` action server is running. This server is typically included with the ROS 2 installation and can be started using the following command: + +```bash +ros2 run action_tutorials_cpp fibonacci_action_server +``` +If the server is not available, you can follow the official ROS 2 tutorial [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html#writing-an-action-server) to compile and run it. + +Once the action server is up, you can run the **Fibonacci BehaviorTree action client** node with: +``` +ros2 run bt_tutorials_ros2 bt_tutorials_ros2_action_client_node +``` + +### AddTwoInts Service Client +Ensure the `/add_two_ints` service server is running. You can implement, compile, and run the server following the official ROS 2 tutorial [here](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html). For example, you can run the server with: +``` +ros2 run cpp_srvcli server +``` + +Once the service server is up, you can run the **AddTwoInts BehaviorTree service client** node with: +``` +ros2 run bt_tutorials_ros2 bt_tutorials_ros2_service_client_node +``` + +### Author +Janak Panthi (Crasun Jans) diff --git a/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp b/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp new file mode 100644 index 0000000..3e6c128 --- /dev/null +++ b/bt_tutorials_ros2/include/bt_tutorials_ros2/bt_tutorials_ros2_action_client.hpp @@ -0,0 +1,115 @@ +/** + * @file bt_tutorials_ros2_action_client.hpp + * @brief Header file for the FibonacciActionNode, a custom BehaviorTree node for interacting with the official ROS 2 + * "/fibonacci" action server. + * + * @author Janak Panthi (Crasun Jans) + * + * @details Implements the Fibonacci action client tutorial from the "Integration with ROS2" section on the + * official BehaviorTree.CPP website. This node sends goals to a Fibonacci action server, processes its feedback, + * and handles the result. + * + * @license MIT + */ + +#ifndef BT_TUTORIALS_ROS2_ACTION_CLIENT_HPP +#define BT_TUTORIALS_ROS2_ACTION_CLIENT_HPP + +#include // 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"( + + + +