added tree_execution_client node

Author: Jonathan Cacace

behaviortree_ros2/CMakeLists.txt

@@ -53,6 +53,11 @@ add_executable(bt_ros_plugins_to_xml         src/tools/bt_ros_plugins_to_xml.cpp
 include_directories(${behaviortree_cpp_INCLUDE_DIRS} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
 ament_target_dependencies(bt_ros_plugins_to_xml behaviortree_cpp rclcpp)
 
+
+add_executable(tree_execution_client src/tools/client/tree_execution_client.cpp)
+ament_target_dependencies(tree_execution_client ${THIS_PACKAGE_DEPS})
+ament_target_dependencies(tree_execution_client behaviortree_cpp rclcpp)
+
 # Build btros2 plugin library
 add_library(${PROJECT_NAME}_nodemodels SHARED src/plugin.cpp)
 target_include_directories(${PROJECT_NAME}_nodemodels PRIVATE
@@ -68,6 +73,11 @@ ament_target_dependencies(${PROJECT_NAME}_nodemodels ${THIS_PACKAGE_DEPS})
 install(TARGETS bt_ros_plugins_to_xml
         DESTINATION lib/${PROJECT_NAME} )
 
+install(TARGETS tree_execution_client
+        DESTINATION lib/${PROJECT_NAME} )
+
+
+
 install(DIRECTORY include/  DESTINATION include/)
 
 ament_export_targets(${PROJECT_NAME}Targets  HAS_LIBRARY_TARGET)

behaviortree_ros2/src/tools/client/tree_execution_client.cpp

@@ -0,0 +1,102 @@
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <btcpp_ros2_interfaces/action/execute_tree.hpp>
+
+class TreeExecutionClient : public rclcpp::Node
+{
+public:
+    using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree;
+    using GoalHandleExecuteTree = rclcpp_action::ClientGoalHandle<ExecuteTree>;
+
+    explicit TreeExecutionClient(const std::string& target_tree, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+        : Node("tree_execution_client", options)
+    {
+        action_client_ = rclcpp_action::create_client<ExecuteTree>(this, "/bt_action_server_example");
+
+        if (!action_client_->wait_for_action_server(std::chrono::seconds(10)))
+        {
+            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
+            rclcpp::shutdown();
+            return;
+        }
+
+        if (target_tree.empty())
+        {
+            RCLCPP_ERROR(this->get_logger(), "Target tree name must be specified");
+            rclcpp::shutdown();
+            return;
+        }
+
+        send_goal(target_tree);
+    }
+
+private:
+    rclcpp_action::Client<ExecuteTree>::SharedPtr action_client_;
+
+    void send_goal(const std::string &target_tree)
+    {
+        auto goal_msg = ExecuteTree::Goal();
+        goal_msg.target_tree = target_tree;
+        goal_msg.payload = ""; // Optional payload can be set here
+
+        RCLCPP_INFO(this->get_logger(), "Sending goal to execute tree: %s", target_tree.c_str());
+
+        auto send_goal_options = rclcpp_action::Client<ExecuteTree>::SendGoalOptions();
+        
+        send_goal_options.goal_response_callback =
+            [this](GoalHandleExecuteTree::SharedPtr goal_handle) {
+                if (!goal_handle)
+                {
+                    RCLCPP_ERROR(this->get_logger(), "Goal was rejected by the server");
+                }
+                else
+                {
+                    RCLCPP_INFO(this->get_logger(), "Goal accepted by the server, waiting for result");
+                }
+            };
+
+        send_goal_options.feedback_callback =
+            [this](GoalHandleExecuteTree::SharedPtr, const std::shared_ptr<const ExecuteTree::Feedback> feedback) {
+                RCLCPP_INFO(this->get_logger(), "Feedback received: %s", feedback->message.c_str());
+            };
+
+        send_goal_options.result_callback =
+            [this](const GoalHandleExecuteTree::WrappedResult &result) {
+                switch (result.code)
+                {
+                case rclcpp_action::ResultCode::SUCCEEDED:
+                    RCLCPP_INFO(this->get_logger(), "Result received: Status=%d, Message=%s",
+                                result.result->node_status, result.result->return_message.c_str());
+                    break;
+                case rclcpp_action::ResultCode::ABORTED:
+                    RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+                    break;
+                case rclcpp_action::ResultCode::CANCELED:
+                    RCLCPP_WARN(this->get_logger(), "Goal was canceled");
+                    break;
+                default:
+                    RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+                    break;
+                }
+                rclcpp::shutdown();
+            };
+            
+        action_client_->async_send_goal(goal_msg, send_goal_options);
+    }
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    
+    if (argc < 2) {
+        RCLCPP_ERROR(rclcpp::get_logger("tree_execution_client"), "Usage: ros2 run package_name node_name <target_tree_name>");
+        return 1;
+    }
+    
+    std::string target_tree = argv[1];
+    auto node = std::make_shared<TreeExecutionClient>(target_tree);
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}