From 5c8ed91ccfafc5eecf456f8b409c40c15619af53 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 6 Mar 2026 20:52:01 +0100 Subject: [PATCH] Added initial batch of behaviors --- toid_bt/CMakeLists.txt | 62 ++++++ toid_bt/LICENSE | 202 ++++++++++++++++++ toid_bt/behavior_trees/behavior1.xml | 33 +++ .../behavior_trees/calibration_routines.xml | 53 +++++ toid_bt/behavior_trees/toid_behaviors.btproj | 39 ++++ toid_bt/include/toid_bt/bt_executor.hpp | 34 +++ toid_bt/include/toid_bt/plugin.hpp | 8 + .../toid_bt/plugins/empty_srv_action.hpp | 30 +++ .../toid_bt/plugins/end_calib_action.hpp | 56 +++++ .../toid_bt/plugins/move_coords_action.hpp | 65 ++++++ .../include/toid_bt/plugins/rotate_action.hpp | 63 ++++++ .../toid_bt/plugins/rotate_towards_action.hpp | 70 ++++++ toid_bt/launch/bt.launch.py | 41 ++++ toid_bt/package.xml | 31 +++ toid_bt/params/bt_params.yaml | 9 + toid_bt/src/bt_executor.cpp | 103 +++++++++ 16 files changed, 899 insertions(+) create mode 100644 toid_bt/CMakeLists.txt create mode 100644 toid_bt/LICENSE create mode 100644 toid_bt/behavior_trees/behavior1.xml create mode 100644 toid_bt/behavior_trees/calibration_routines.xml create mode 100644 toid_bt/behavior_trees/toid_behaviors.btproj create mode 100644 toid_bt/include/toid_bt/bt_executor.hpp create mode 100644 toid_bt/include/toid_bt/plugin.hpp create mode 100644 toid_bt/include/toid_bt/plugins/empty_srv_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/end_calib_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/move_coords_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/rotate_action.hpp create mode 100644 toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp create mode 100644 toid_bt/launch/bt.launch.py create mode 100644 toid_bt/package.xml create mode 100644 toid_bt/params/bt_params.yaml create mode 100644 toid_bt/src/bt_executor.cpp diff --git a/toid_bt/CMakeLists.txt b/toid_bt/CMakeLists.txt new file mode 100644 index 0000000..e0a8cf9 --- /dev/null +++ b/toid_bt/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_bt) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(PACKAGE_DEPS + rclcpp + angles + behaviortree_cpp + behaviortree_ros2 + btcpp_ros2_interfaces + nav2_util + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + toid_msgs + std_msgs + std_srvs +) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +set(SOURCES + src/bt_executor.cpp +) + +add_executable(toid_bt ${SOURCES}) + +target_include_directories( + toid_bt + PRIVATE + include +) + +ament_target_dependencies(toid_bt ${PACKAGE_DEPS}) + +install(TARGETS toid_bt DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY + launch + params + behavior_trees + DESTINATION share/${PROJECT_NAME}/ +) + +target_compile_features( + toid_bt PUBLIC + c_std_99 + cxx_std_17 +) + + +ament_package() + diff --git a/toid_bt/LICENSE b/toid_bt/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/toid_bt/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml new file mode 100644 index 0000000..e3023e1 --- /dev/null +++ b/toid_bt/behavior_trees/behavior1.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + Action server name + + + + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml new file mode 100644 index 0000000..2a8d556 --- /dev/null +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + Action server name + + + + + + Action server name + + + Service name + + + + diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj new file mode 100644 index 0000000..e8854a1 --- /dev/null +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -0,0 +1,39 @@ + + + + + + + + Service name + + + + + + Action server name + + + + + + Action server name + + + + + + + Action server name + + + + + + Service name + + + Service name + + + diff --git a/toid_bt/include/toid_bt/bt_executor.hpp b/toid_bt/include/toid_bt/bt_executor.hpp new file mode 100644 index 0000000..7e95045 --- /dev/null +++ b/toid_bt/include/toid_bt/bt_executor.hpp @@ -0,0 +1,34 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" + +#include "behaviortree_ros2/tree_execution_server.hpp" +#include "behaviortree_cpp/loggers/bt_cout_logger.h" + +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace toid { + class TreeExecutor : public BT::TreeExecutionServer { + public: + TreeExecutor(const rclcpp::NodeOptions opts); + void onTreeCreated(BT::Tree& tree) override; + + void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override; + + std::optional onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override; + + void position(geometry_msgs::msg::PoseStamped &pose); + + std::string describeCustomNodes(); + + private: + std::shared_ptr logger_cout_; + tf2_ros::Buffer::SharedPtr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string base_frame_; + std::string global_frame_; + }; +} \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugin.hpp b/toid_bt/include/toid_bt/plugin.hpp new file mode 100644 index 0000000..6a7fc85 --- /dev/null +++ b/toid_bt/include/toid_bt/plugin.hpp @@ -0,0 +1,8 @@ +#pragma once +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace toid { + using PoseFunc = std::function; +} \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp b/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp new file mode 100644 index 0000000..0719f96 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/empty_srv_action.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class EmptySrvNode : public BT::RosServiceNode +{ +public: + EmptySrvNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + bool setRequest(typename Request::SharedPtr &) override { return true; } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp new file mode 100644 index 0000000..35127fc --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/srv/send_double.hpp" + +namespace toid +{ + +class EndCalibNode : public BT::RosServiceNode +{ +public: + EndCalibNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosServiceNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("width"), BT::InputPort("count", 1, {}), + BT::OutputPort("new_width"), + //BT::InputPort("options"), + }); + } + + bool setRequest(typename Request::SharedPtr & request) override + { + auto width = getInput("width").value(); + auto count = getInput("count").value(); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + double theta = tf2::getYaw(pose.pose.orientation); + double new_width = width * (1 + (theta / (2 * M_PI * count))); + request->data = new_width; + + setOutput("new_width", new_width); + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp new file mode 100644 index 0000000..6bc5b6c --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" + +namespace toid +{ + +class MovePointNode : public BT::RosActionNode +{ +public: + MovePointNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + std::cout << "wtf? " << params.default_port_value << std::endl; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("theta"), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x_goal = getInput("x"); + auto y_goal = getInput("y"); + auto theta = getInput("theta"); + goal.x = x_goal.value(); + goal.y = y_goal.value(); + if (!theta.has_value()) { + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + goal.theta = tf2::getYaw(pose.pose.orientation); + } else { + goal.theta = angles::from_degrees(theta.value_or(0)); + } + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/rotate_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_action.hpp new file mode 100644 index 0000000..be76e20 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_rotate.hpp" + +namespace toid +{ + +class RotateNode : public BT::RosActionNode +{ +public: + RotateNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("angle", {}), + BT::InputPort("min_angle", 0, {}), + BT::InputPort("radians", false, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto angle = getInput("angle"); + auto min_angle = getInput("min_angle"); + auto radians = getInput("radians"); + goal.angle = angle.value(); + goal.min_angle = min_angle.value(); + if (!radians.value()) { + goal.angle = angles::from_degrees(goal.angle); + goal.min_angle = angles::from_degrees(goal.min_angle); + } + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp new file mode 100644 index 0000000..fb39f24 --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_rotate.hpp" + +namespace toid +{ + +class RotateTowardsNode : public BT::RosActionNode +{ +public: + RotateTowardsNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x", {}), + BT::InputPort("y", {}), + BT::InputPort("min_angle", 0, {}), + BT::InputPort("radians", false, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x = getInput("x").value(); + auto y = getInput("y").value(); + auto min_angle = getInput("min_angle"); + auto radians = getInput("radians"); + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); + goal.min_angle = min_angle.value(); + + if (!radians.value()) { + goal.min_angle = angles::from_degrees(goal.min_angle); + } + + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + } + + BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override + { + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/launch/bt.launch.py b/toid_bt/launch/bt.launch.py new file mode 100644 index 0000000..ab6f55d --- /dev/null +++ b/toid_bt/launch/bt.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + + pkg_share = FindPackageShare("").find('toid_bt') + bt_params = os.path.join(pkg_share, 'params', 'bt_params.yaml') + + visualize = LaunchConfiguration("visualize") + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='False', + description="Whether to launch rviz2" + ) + + use_mock = LaunchConfiguration("use_mock") + use_mock_arg = DeclareLaunchArgument( + 'use_mock', + default_value='True', + description="Whether to use mock controller" + ) + + bt_executor = Node( + package='toid_bt', + executable='toid_bt', + output='screen', + emulate_tty=True, + parameters=[bt_params], + ) + + return LaunchDescription([ + visualize_arg, + use_mock_arg, + bt_executor, + ]) + diff --git a/toid_bt/package.xml b/toid_bt/package.xml new file mode 100644 index 0000000..146dca5 --- /dev/null +++ b/toid_bt/package.xml @@ -0,0 +1,31 @@ + + + + toid_bt + 0.0.0 + TODO: Package description + pimpest + Apache-2.0 + + ament_cmake + + rclcpp + angles + behaviortree_ros2 + btcpp_ros2_interfaces + nav2_util + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + toid_msgs + std_msgs + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/toid_bt/params/bt_params.yaml b/toid_bt/params/bt_params.yaml new file mode 100644 index 0000000..5ac5c62 --- /dev/null +++ b/toid_bt/params/bt_params.yaml @@ -0,0 +1,9 @@ +toid_bt: + ros__parameters: + action_name: "bt_run" + tick_frequency: 100 + groot2_port: 8420 + ros_plugins_timeout: 1000 + + behavior_trees: + - toid_bt/behavior_trees \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp new file mode 100644 index 0000000..8679486 --- /dev/null +++ b/toid_bt/src/bt_executor.cpp @@ -0,0 +1,103 @@ +#include "toid_bt/bt_executor.hpp" + +#include "behaviortree_cpp/xml_parsing.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "toid_bt/plugins/empty_srv_action.hpp" +#include "toid_bt/plugins/end_calib_action.hpp" +#include "toid_bt/plugins/move_coords_action.hpp" +#include "toid_bt/plugins/rotate_action.hpp" +#include "toid_bt/plugins/rotate_towards_action.hpp" + +namespace toid +{ +TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) +: BT::TreeExecutionServer(std::make_shared("toid_bt", opts)) +{ + /* + executeRegistration(); + std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl; + tf_buffer_ = std::make_shared(node()->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + */ + + nav2_util::declare_parameter_if_not_declared( + node(), "base_frame", rclcpp::ParameterValue("base_footprint")); + node()->get_parameter("base_frame", base_frame_); + + nav2_util::declare_parameter_if_not_declared( + node(), "global_frame", rclcpp::ParameterValue("map")); + node()->get_parameter("global_frame", global_frame_); +} + +void TreeExecutor::onTreeCreated(BT::Tree & tree) +{ + logger_cout_ = std::make_shared(tree); +} + +void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) +{ + PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); }; + rclcpp::Node::SharedPtr nh = node(); + factory.registerNodeType( + "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); + + factory.registerNodeType( + "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); + + factory.registerNodeType( + "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); + + factory.registerNodeType( + "ZeroOdom", BT::RosNodeParams(nh, "/zero")); + + factory.registerNodeType( + "EndCalib", BT::RosNodeParams(nh, "/end_calib")); + + std::cout << describeCustomNodes() << std::endl; + +} + +void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) +{ + nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_); +} + +std::optional TreeExecutor::onTreeExecutionCompleted( + BT::NodeStatus status, bool was_cancelled) +{ + (void)status; + logger_cout_.reset(); + + if (was_cancelled) { + std::cout << "Behavior tree was cancled" << std::endl; + } + + return std::nullopt; +} + +std::string TreeExecutor::describeCustomNodes() { return BT::writeTreeNodesModelXML(factory()); } + +} // namespace toid + +int main(const int argc, const char * const * argv) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto bt_exec_server = std::make_shared(options); + + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250)); + + executor.add_node(bt_exec_server->node()); + executor.spin(); + executor.remove_node(bt_exec_server->node()); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file