diff --git a/CHANGELOG.md b/CHANGELOG.md index 7e7ac5b..6e64f3c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/). 8. Added mission metrics node +9. Add a behavior tree as a managing subsystem for SUAVE and SUAVE extended to serve as a baseline for comparison + ### Changed 1. Ardusub, mavros, ros_gz, mc_mros_reasoner, mc_mdl_tomasys, mros_ontology verions diff --git a/docker/dockerfile-suave b/docker/dockerfile-suave index 743844c..c8339bc 100644 --- a/docker/dockerfile-suave +++ b/docker/dockerfile-suave @@ -113,8 +113,9 @@ RUN vcs import src < suave.rosinstall --recursive RUN rm -rf $HOME/suave_ws/src/suave/ COPY --chown=kasm-user:kasm-user suave/ $HOME/suave_ws/src/suave/suave/ +COPY --chown=kasm-user:kasm-user suave_managing/ $HOME/suave_ws/src/suave/suave_managing/ +COPY --chown=kasm-user:kasm-user suave_metrics/ $HOME/suave_ws/src/suave/suave_metrics/ COPY --chown=kasm-user:kasm-user suave_missions/ $HOME/suave_ws/src/suave/suave_missions/ -COPY --chown=kasm-user:kasm-user suave_metacontrol/ $HOME/suave_ws/src/suave/suave_metacontrol/ COPY --chown=kasm-user:kasm-user suave_msgs/ $HOME/suave_ws/src/suave/suave_msgs/ # Install suave deps diff --git a/suave/config/suave_modes.yaml b/suave/config/suave_modes.yaml index 823d6a1..95a250e 100644 --- a/suave/config/suave_modes.yaml +++ b/suave/config/suave_modes.yaml @@ -76,3 +76,26 @@ f_maintain_motion_node: __DEFAULT__: ros__parameters: random_param: 2.0 + +generate_recharge_path: + ros__parameters: + type: system + parts: + generate_recharge_path_node + modes: + __DEFAULT__: + generate_recharge_path_node: inactive + normal: + generate_recharge_path_node: active.__DEFAULT__ + inactive: + generate_recharge_path_node: inactive + fd_unground: + generate_recharge_path_node: inactive + +generate_recharge_path_node: + ros__parameters: + type: node + modes: + __DEFAULT__: + ros__parameters: + random_param: 2.0 diff --git a/suave/suave/recharge_battery_lc.py b/suave/suave/recharge_battery_lc.py index 68ec3d7..f91ece4 100644 --- a/suave/suave/recharge_battery_lc.py +++ b/suave/suave/recharge_battery_lc.py @@ -134,7 +134,7 @@ def main(args=None): rclpy.init(args=args) try: executor = MultiThreadedExecutor() - lc_node = RechargeBattery('recharge_battery') + lc_node = RechargeBattery('generate_recharge_path_node') executor.add_node(lc_node) try: executor.spin() diff --git a/suave_managing/suave_bt/CMakeLists.txt b/suave_managing/suave_bt/CMakeLists.txt new file mode 100644 index 0000000..0db75e4 --- /dev/null +++ b/suave_managing/suave_bt/CMakeLists.txt @@ -0,0 +1,130 @@ +cmake_minimum_required(VERSION 3.8) +project(suave_bt) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") +# list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}") + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(system_modes_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +set(dependencies + ament_index_cpp + behaviortree_cpp + mavros_msgs + rclcpp + std_msgs + std_srvs + system_modes_msgs + diagnostic_msgs +) + +# include_directories( +# include +# ) + +add_library(suave_mission SHARED src/suave_bt/suave_mission.cpp) +add_library(is_pipeline_found SHARED src/suave_bt/is_pipeline_found.cpp) +add_library(is_pipeline_inspected SHARED src/suave_bt/is_pipeline_inspected.cpp) +add_library(condition_water_visibility SHARED src/suave_bt/condition_water_visibility.cpp) +add_library(condition_thrusters_ok SHARED src/suave_bt/condition_thrusters_ok.cpp) +add_library(condition_battery_level SHARED src/suave_bt/condition_battery_level.cpp) + +add_library(action_arm_thrusters SHARED src/suave_bt/action_arm_thrusters.cpp) +add_library(action_set_guided_mode SHARED src/suave_bt/action_set_guided_mode.cpp) +add_library(action_search_pipeline SHARED src/suave_bt/action_search_pipeline.cpp) +add_library(action_inspect_pipeline SHARED src/suave_bt/action_inspect_pipeline.cpp) +add_library(action_recharge_battery SHARED src/suave_bt/action_recharge_battery.cpp) +add_library(action_change_mode SHARED src/suave_bt/action_change_mode.cpp) + + +list(APPEND plugin_libs + suave_mission + is_pipeline_found + is_pipeline_inspected + condition_water_visibility + condition_thrusters_ok + condition_battery_level + action_arm_thrusters + action_set_guided_mode + action_search_pipeline + action_inspect_pipeline + action_recharge_battery + action_change_mode +) + +foreach(bt_plugin ${plugin_libs}) + # ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_features(${bt_plugin} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + target_include_directories(${bt_plugin} PUBLIC + $ + $) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE "SUAVE_BT_BUILDING_LIBRARY") +endforeach() + +add_executable(suave_bt src/suave_bt.cpp) +ament_target_dependencies(suave_bt ${dependencies}) +target_link_libraries(suave_bt ${plugin_libs}) + +add_executable(suave_bt_extended src/suave_bt_extended.cpp) +ament_target_dependencies(suave_bt_extended ${dependencies}) +target_link_libraries(suave_bt_extended ${plugin_libs}) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(TARGETS + suave_bt + suave_bt_extended + DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS ${plugin_libs} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# add config, launch etc, whatever is needed +install(DIRECTORY + bts + launch + DESTINATION share/${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_export_include_directories(include) + +ament_export_libraries( + ${plugin_libs} +) +ament_export_targets( + export_${PROJECT_NAME} +) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/suave_managing/suave_bt/LICENSE b/suave_managing/suave_bt/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/suave_managing/suave_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/suave_managing/suave_bt/bts/suave.xml b/suave_managing/suave_bt/bts/suave.xml new file mode 100644 index 0000000..f0911be --- /dev/null +++ b/suave_managing/suave_bt/bts/suave.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/suave_managing/suave_bt/bts/suave_extended.xml b/suave_managing/suave_bt/bts/suave_extended.xml new file mode 100644 index 0000000..f64828d --- /dev/null +++ b/suave_managing/suave_bt/bts/suave_extended.xml @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/suave_managing/suave_bt/include/suave_bt/action_arm_thrusters.hpp b/suave_managing/suave_bt/include/suave_bt/action_arm_thrusters.hpp new file mode 100644 index 0000000..e25bf69 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_arm_thrusters.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__ARM_THRUSTERS_HPP_ +#define SUAVE_BT__ARM_THRUSTERS_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "mavros_msgs/srv/command_bool.hpp" +#include "suave_bt/suave_mission.hpp" + + +namespace suave_bt +{ + +class ArmThrusters : public BT::StatefulActionNode{ + +public: + ArmThrusters(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus onRunning() override; + + BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;}; + + void onHalted() override {}; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +protected: + suave_bt::SuaveMission::SharedPtr node_; + rclcpp::Client::SharedPtr arm_motors_cli_; +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__ARM_THRUSTERS_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/action_change_mode.hpp b/suave_managing/suave_bt/include/suave_bt/action_change_mode.hpp new file mode 100644 index 0000000..e1b7c80 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_change_mode.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__CHANGE_MODE_HPP_ +#define SUAVE_BT__CHANGE_MODE_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "suave_bt/suave_mission.hpp" +#include "system_modes_msgs/srv/change_mode.hpp" +#include "system_modes_msgs/srv/get_mode.hpp" + + +namespace suave_bt +{ + +class ChangeMode : public BT::SyncActionNode{ + +public: + ChangeMode(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("node_name"), + BT::InputPort("mode_name"), + }); + } + +protected: + suave_bt::SuaveMission::SharedPtr node_; + std::string node_name_; + std::shared_ptr> previous_modes_; + rclcpp::Client::SharedPtr change_mode_cli_; + rclcpp::Client::SharedPtr get_mode_cli_; +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__CHANGE_MODE_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/action_inspect_pipeline.hpp b/suave_managing/suave_bt/include/suave_bt/action_inspect_pipeline.hpp new file mode 100644 index 0000000..9ee718f --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_inspect_pipeline.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__INSPECT_PIPELINE_HPP_ +#define SUAVE_BT__INSPECT_PIPELINE_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + +class InspectPipeline : public BT::StatefulActionNode{ + +public: + InspectPipeline(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;}; + + BT::NodeStatus onRunning() override; + + void onHalted() override {}; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + std::shared_ptr node_; + bool pipeline_inspected_; + rclcpp::Subscription::SharedPtr pipeline_inspected_sub_; + void pipeline_inspected_cb(const std_msgs::msg::Bool &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__INSPECT_PIPELINE_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp b/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp new file mode 100644 index 0000000..99d7fdf --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__RECHARGE_BATTERY_HPP_ +#define SUAVE_BT__RECHARGE_BATTERY_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + +class RechargeBattery : public BT::StatefulActionNode{ + +public: + RechargeBattery(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;}; + + BT::NodeStatus onRunning() override; + + void onHalted() override {}; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + std::shared_ptr node_; + + bool recharged_; + rclcpp::Subscription::SharedPtr battery_level_sub_; + void battery_level_cb(const std_msgs::msg::Bool &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__RECHARGE_BATTERY_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/action_search_pipeline.hpp b/suave_managing/suave_bt/include/suave_bt/action_search_pipeline.hpp new file mode 100644 index 0000000..8ee3d6c --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_search_pipeline.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__SEARCH_PIPELINE_HPP_ +#define SUAVE_BT__SEARCH_PIPELINE_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + +class SearchPipeline : public BT::StatefulActionNode{ + +public: + SearchPipeline(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus onStart() override; + + BT::NodeStatus onRunning() override; + + void onHalted() override {}; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + std::shared_ptr node_; + + bool pipeline_detected_; + rclcpp::Subscription::SharedPtr pipeline_detection_sub_; + void pipeline_detected_cb(const std_msgs::msg::Bool &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__SEARCH_PIPELINE_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/action_set_guided_mode.hpp b/suave_managing/suave_bt/include/suave_bt/action_set_guided_mode.hpp new file mode 100644 index 0000000..f0faec3 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/action_set_guided_mode.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__SET_GUIDED_MODE_HPP_ +#define SUAVE_BT__SET_GUIDED_MODE_HPP_ + +#include +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "mavros_msgs/msg/state.hpp" +#include "mavros_msgs/srv/set_mode.hpp" + +#include "suave_bt/suave_mission.hpp" + +namespace suave_bt +{ + +class SetGuidedMode : public BT::StatefulActionNode{ + +public: + SetGuidedMode(const std::string& name, const BT::NodeConfig & conf); + + BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;}; + + void onHalted() override {}; + + BT::NodeStatus onRunning() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +protected: + std::string mode_; + suave_bt::SuaveMission::SharedPtr _node; + rclcpp::Client::SharedPtr set_guided_cli_; + rclcpp::Subscription::SharedPtr mavros_state_sub_; + void state_cb(const mavros_msgs::msg::State &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__SET_GUIDED_MODE_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/condition_battery_level.hpp b/suave_managing/suave_bt/include/suave_bt/condition_battery_level.hpp new file mode 100644 index 0000000..ffd7fa7 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/condition_battery_level.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__CONDITION_BATTERY_LEVEL_HPP_ +#define SUAVE_BT__CONDITION_BATTERY_LEVEL_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::chrono_literals; + +namespace suave_bt +{ + +class BatteryLevel : public BT::ConditionNode +{ +public: + explicit BatteryLevel( + const std::string & xml_tag_name, + const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("threshold"), + }); + } + +private: + std::shared_ptr node_; + + float battery_level_; + rclcpp::Subscription::SharedPtr diagnostics_sub_; + void diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg); +}; + +} //namespace suave_bt + +#endif // SUAVE_BT__CONDITION_BATTERY_LEVEL_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/condition_thrusters_ok.hpp b/suave_managing/suave_bt/include/suave_bt/condition_thrusters_ok.hpp new file mode 100644 index 0000000..57ab41c --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/condition_thrusters_ok.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__CONDITION_THRUSTERS_OK_HPP_ +#define SUAVE_BT__CONDITION_THRUSTERS_OK_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::chrono_literals; + +namespace suave_bt +{ + +class ThrustersOk : public BT::ConditionNode +{ +public: + explicit ThrustersOk( + const std::string & xml_tag_name, + const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + std::shared_ptr node_; + + std::map thrusters_ok_; + rclcpp::Subscription::SharedPtr diagnostics_sub_; + void diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg); +}; + +} //namespace suave_bt + +#endif // SUAVE_BT__CONDITION_THRUSTERS_OK_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/condition_water_visibility.hpp b/suave_managing/suave_bt/include/suave_bt/condition_water_visibility.hpp new file mode 100644 index 0000000..4ffb23b --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/condition_water_visibility.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__CONDITION_WATER_VISIBILITY_HPP_ +#define SUAVE_BT__CONDITION_WATER_VISIBILITY_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +#include "suave_bt/suave_mission.hpp" + +using namespace std::chrono_literals; + +namespace suave_bt +{ + +class WaterVisibility : public BT::ConditionNode +{ +public: + explicit WaterVisibility( + const std::string & xml_tag_name, + const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("threshold"), + }); + } + +private: + std::shared_ptr node_; + + float water_visibility_; + rclcpp::Subscription::SharedPtr diagnostics_sub_; + void diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg); +}; + +} //namespace suave_bt + +#endif // SUAVE_BT__CONDITION_WATER_VISIBILITY_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/is_pipeline_found.hpp b/suave_managing/suave_bt/include/suave_bt/is_pipeline_found.hpp new file mode 100644 index 0000000..d674ccf --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/is_pipeline_found.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__PIPELINE_FOUND_HPP_ +#define SUAVE_BT__PIPELINE_FOUND_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "suave_bt/suave_mission.hpp" + +namespace suave_bt +{ + +class IsPipelineFound : public BT::ConditionNode +{ +public: + explicit IsPipelineFound(const std::string & xml_tag_name, + const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + bool _pipeline_detected; + + suave_bt::SuaveMission::SharedPtr _node; + rclcpp::Subscription::SharedPtr pipeline_detection_sub_; + + void pipeline_detected_cb(const std_msgs::msg::Bool &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__PIPELINE_FOUND_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/is_pipeline_inspected.hpp b/suave_managing/suave_bt/include/suave_bt/is_pipeline_inspected.hpp new file mode 100644 index 0000000..6d32443 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/is_pipeline_inspected.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__PIPELINE_INSPECTED_HPP_ +#define SUAVE_BT__PIPELINE_INSPECTED_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "suave_bt/suave_mission.hpp" + +namespace suave_bt +{ + +class IsPipelineInspected : public BT::ConditionNode +{ +public: + explicit IsPipelineInspected(const std::string & xml_tag_name, + const BT::NodeConfig & conf); + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + }); + } + +private: + bool _pipeline_inspected; + + suave_bt::SuaveMission::SharedPtr _node; + rclcpp::Subscription::SharedPtr pipeline_inspected_sub_; + + void pipeline_inspected_cb(const std_msgs::msg::Bool &msg); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__PIPELINE_INSPECTED_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/suave_mission.hpp b/suave_managing/suave_bt/include/suave_bt/suave_mission.hpp new file mode 100644 index 0000000..b130245 --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/suave_mission.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 SUAVE_BT__SUAVE_MISSION_HPP_ +#define SUAVE_BT__SUAVE_MISSION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_srvs/srv/empty.hpp" + +namespace suave_bt +{ + +class SuaveMission : public rclcpp::Node{ + +public: + SuaveMission(std::string none_name); + + bool time_limit_reached(); + void set_search_started(); + bool is_mission_aborted(){return mission_aborted_;}; + void finish_mission(); + +private: + rclcpp::Time start_time_; + bool search_started_ = false; + int time_limit_; + rclcpp::Client::SharedPtr save_mission_results_cli; + + bool mission_aborted_; + + rclcpp::CallbackGroup::SharedPtr time_limit_timer_cb_group_; + rclcpp::TimerBase::SharedPtr time_limit_timer_; + + void time_limit_cb(); + bool request_save_mission_results(); +}; + +} // namespace suave_bt + +#endif // SUAVE_BT__SUAVE_MISSION_HPP_ diff --git a/suave_managing/suave_bt/include/suave_bt/visibility_control.h b/suave_managing/suave_bt/include/suave_bt/visibility_control.h new file mode 100644 index 0000000..a8e4a3b --- /dev/null +++ b/suave_managing/suave_bt/include/suave_bt/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef SUAVE_BT__VISIBILITY_CONTROL_H_ +#define SUAVE_BT__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define SUAVE_BT_EXPORT __attribute__ ((dllexport)) + #define SUAVE_BT_IMPORT __attribute__ ((dllimport)) + #else + #define SUAVE_BT_EXPORT __declspec(dllexport) + #define SUAVE_BT_IMPORT __declspec(dllimport) + #endif + #ifdef SUAVE_BT_BUILDING_LIBRARY + #define SUAVE_BT_PUBLIC SUAVE_BT_EXPORT + #else + #define SUAVE_BT_PUBLIC SUAVE_BT_IMPORT + #endif + #define SUAVE_BT_PUBLIC_TYPE SUAVE_BT_PUBLIC + #define SUAVE_BT_LOCAL +#else + #define SUAVE_BT_EXPORT __attribute__ ((visibility("default"))) + #define SUAVE_BT_IMPORT + #if __GNUC__ >= 4 + #define SUAVE_BT_PUBLIC __attribute__ ((visibility("default"))) + #define SUAVE_BT_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define SUAVE_BT_PUBLIC + #define SUAVE_BT_LOCAL + #endif + #define SUAVE_BT_PUBLIC_TYPE +#endif + +#endif // SUAVE_BT__VISIBILITY_CONTROL_H_ diff --git a/suave_managing/suave_bt/launch/suave_bt.launch.py b/suave_managing/suave_bt/launch/suave_bt.launch.py new file mode 100644 index 0000000..3cf8838 --- /dev/null +++ b/suave_managing/suave_bt/launch/suave_bt.launch.py @@ -0,0 +1,107 @@ +# Copyright 2024 Gustavo Rezende Silva +# +# 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. +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + mission_type = LaunchConfiguration('mission_type') + result_filename = LaunchConfiguration('result_filename') + mission_config = LaunchConfiguration('mission_config') + + mission_type_arg = DeclareLaunchArgument( + 'mission_type', + default_value='suave', + description='Mission name for logging' + ) + + result_filename_arg = DeclareLaunchArgument( + 'result_filename', + default_value='', + description='Name of the results file' + ) + + mission_config_default = os.path.join( + get_package_share_directory('suave_missions'), + 'config', + 'mission_config.yaml' + ) + + mission_config_arg = DeclareLaunchArgument( + 'mission_config', + default_value=mission_config_default, + description='Mission configuration file' + ) + + pkg_suave_path = get_package_share_directory( + 'suave') + suave_launch_path = os.path.join( + pkg_suave_path, + 'launch', + 'suave.launch.py') + + suave_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(suave_launch_path), + launch_arguments={ + 'task_bridge': 'False'}.items() + ) + + mission_metrics_node = Node( + package='suave_metrics', + executable='mission_metrics', + name='mission_metrics', + parameters=[mission_config, { + 'adaptation_manager': 'bt', + 'mission_name': mission_type, + }], + condition=LaunchConfigurationEquals('result_filename', '') + ) + + mission_metrics_node_override = Node( + package='suave_metrics', + executable='mission_metrics', + name='mission_metrics', + parameters=[mission_config, { + 'adaptation_manager': 'bt', + 'mission_name': mission_type, + 'result_filename': result_filename, + }], + condition=LaunchConfigurationNotEquals('result_filename', '') + ) + + suave_bt_node = Node( + package='suave_bt', + executable='suave_bt', + parameters=[mission_config] + ) + + return LaunchDescription([ + mission_type_arg, + result_filename_arg, + mission_config_arg, + suave_launch, + mission_metrics_node, + mission_metrics_node_override, + suave_bt_node + ]) diff --git a/suave_managing/suave_bt/launch/suave_bt_extended.launch.py b/suave_managing/suave_bt/launch/suave_bt_extended.launch.py new file mode 100644 index 0000000..294fc98 --- /dev/null +++ b/suave_managing/suave_bt/launch/suave_bt_extended.launch.py @@ -0,0 +1,107 @@ +# Copyright 2024 Gustavo Rezende Silva +# +# 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. +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + mission_type = LaunchConfiguration('mission_type') + result_filename = LaunchConfiguration('result_filename') + mission_config = LaunchConfiguration('mission_config') + + mission_type_arg = DeclareLaunchArgument( + 'mission_type', + default_value='suave_extended', + description='Mission name for logging' + ) + + result_filename_arg = DeclareLaunchArgument( + 'result_filename', + default_value='', + description='Name of the results file' + ) + + mission_config_default = os.path.join( + get_package_share_directory('suave_missions'), + 'config', + 'mission_config.yaml' + ) + + mission_config_arg = DeclareLaunchArgument( + 'mission_config', + default_value=mission_config_default, + description='Mission configuration file' + ) + + pkg_suave_path = get_package_share_directory( + 'suave') + suave_launch_path = os.path.join( + pkg_suave_path, + 'launch', + 'suave.launch.py') + + suave_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(suave_launch_path), + launch_arguments={ + 'task_bridge': 'False'}.items() + ) + + mission_metrics_node = Node( + package='suave_metrics', + executable='mission_metrics', + name='mission_metrics', + parameters=[mission_config, { + 'adaptation_manager': 'bt', + 'mission_name': mission_type, + }], + condition=LaunchConfigurationEquals('result_filename', '') + ) + + mission_metrics_node_override = Node( + package='suave_metrics', + executable='mission_metrics', + name='mission_metrics', + parameters=[mission_config, { + 'adaptation_manager': 'bt', + 'mission_name': mission_type, + 'result_filename': result_filename, + }], + condition=LaunchConfigurationNotEquals('result_filename', '') + ) + + suave_bt_node = Node( + package='suave_bt', + executable='suave_bt_extended', + parameters=[mission_config] + ) + + return LaunchDescription([ + mission_type_arg, + result_filename_arg, + mission_config_arg, + suave_launch, + mission_metrics_node, + mission_metrics_node_override, + suave_bt_node + ]) diff --git a/suave_managing/suave_bt/package.xml b/suave_managing/suave_bt/package.xml new file mode 100644 index 0000000..a0e03df --- /dev/null +++ b/suave_managing/suave_bt/package.xml @@ -0,0 +1,26 @@ + + + + suave_bt + 0.0.0 + BT implementation of a managing system for SUAVE + gus + Apache-2.0 + + ament_cmake + + suave + suave_msgs + mavros_msgs + system_modes_msgs + diagnostic_msgs + behaviortree_cpp + behaviortree_cpp_dbgsym + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/suave_managing/suave_bt/src/suave_bt.cpp b/suave_managing/suave_bt/src/suave_bt.cpp new file mode 100644 index 0000000..546adcf --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 Gustavo Rezende Silva +// +// 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 "ament_index_cpp/get_package_share_directory.hpp" + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" + +#include "behaviortree_cpp/loggers/groot2_publisher.h" +#include "behaviortree_cpp/xml_parsing.h" +#include "behaviortree_cpp/json_export.h" + +#include "rclcpp/rclcpp.hpp" + +#include "suave_bt/suave_mission.hpp" +#include "suave_bt/is_pipeline_found.hpp" +#include "suave_bt/is_pipeline_inspected.hpp" +#include "suave_bt/condition_water_visibility.hpp" +#include "suave_bt/condition_thrusters_ok.hpp" + +#include "suave_bt/action_arm_thrusters.hpp" +#include "suave_bt/action_set_guided_mode.hpp" +#include "suave_bt/action_search_pipeline.hpp" +#include "suave_bt/action_recharge_battery.hpp" +#include "suave_bt/action_inspect_pipeline.hpp" +#include "suave_bt/action_change_mode.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = std::make_shared("mission_node"); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + factory.registerNodeType("search_pipeline"); + factory.registerNodeType("inspect_pipeline"); + factory.registerNodeType("recharge"); + + factory.registerNodeType("arm_thrusters"); + factory.registerNodeType("set_guided_mode"); + factory.registerNodeType("change_mode"); + + factory.registerNodeType("water_visibility"); + factory.registerNodeType("thrusters_ok"); + + factory.registerSimpleCondition("is_mission_aborted", [&](const auto&) { return node->is_mission_aborted() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; }); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("suave_bt"); + std::string xml_file = pkgpath + "/bts/suave.xml"; + + std::map previous_modes; + previous_modes["f_generate_search_path"] = ""; + previous_modes["f_follow_pipeline"] = ""; + previous_modes["f_maintain_motion"] = ""; + + auto blackboard = BT::Blackboard::create(); + blackboard->set>("node", node); + blackboard->set>>( + "previous_modes", + std::make_shared>(previous_modes)); + + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + // Connect the Groot2Publisher. This will allow Groot2 to + // get the tree and poll status updates. + const unsigned port = 1667; + BT::Groot2Publisher publisher(tree, port); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + std::thread t([&executor]() { + executor.spin(); + }); + + bool finish = false; + while (!finish & rclcpp::ok()) { + finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + std::this_thread::sleep_for(100ms); + } + + if(!node->is_mission_aborted()) node->finish_mission(); + std::this_thread::sleep_for(1s); + + executor.cancel(); + t.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/suave_managing/suave_bt/src/suave_bt/action_arm_thrusters.cpp b/suave_managing/suave_bt/src/suave_bt/action_arm_thrusters.cpp new file mode 100644 index 0000000..cd7ec32 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/action_arm_thrusters.cpp @@ -0,0 +1,58 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/action_arm_thrusters.hpp" + + +namespace suave_bt +{ + using namespace std::placeholders; + using namespace std::chrono_literals; + + ArmThrusters::ArmThrusters( + const std::string& name, const BT::NodeConfig & conf) + : BT::StatefulActionNode(name, conf) + { + node_ = config().blackboard->get>("node"); + arm_motors_cli_ = node_->create_client( + "mavros/cmd/arming"); + } + + BT::NodeStatus ArmThrusters::onRunning(){ + + if(arm_motors_cli_->service_is_ready()){ + auto request = std::make_shared(); + request->value = true; + auto arm_motors_result_ = arm_motors_cli_->async_send_request(request); + + if (arm_motors_result_.wait_for(1s) == std::future_status::ready) + { + auto arm_result_ = arm_motors_result_.get(); + if(!arm_result_->success){ + return BT::NodeStatus::FAILURE; + } + + if(arm_result_->success){ + RCLCPP_INFO(node_->get_logger(), "Thrusters armed!"); + return BT::NodeStatus::SUCCESS; + } + } else { + return BT::NodeStatus::FAILURE; + } + } + + RCLCPP_INFO(node_->get_logger(), "mavros/cmd/arming service not available, waiting again..."); + return BT::NodeStatus::RUNNING; + } +} //namespace suave_bt diff --git a/suave_managing/suave_bt/src/suave_bt/action_change_mode.cpp b/suave_managing/suave_bt/src/suave_bt/action_change_mode.cpp new file mode 100644 index 0000000..906b0b3 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/action_change_mode.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/action_change_mode.hpp" + + +namespace suave_bt +{ + using namespace std::placeholders; + using namespace std::chrono_literals; + + ChangeMode::ChangeMode( + const std::string& name, const BT::NodeConfig & conf) + : BT::SyncActionNode(name, conf) + { + getInput("node_name", node_name_); + + node_ = config().blackboard->get>("node"); + previous_modes_ = config().blackboard->get>>("previous_modes"); + + change_mode_cli_ = node_->create_client( + node_name_ + "/change_mode"); + get_mode_cli_ = node_->create_client( + node_name_ + "/get_mode"); + } + + BT::NodeStatus ChangeMode::tick(){ + std::string mode_name; + getInput("mode_name", mode_name); + + if(previous_modes_->at(node_name_) == mode_name) return BT::NodeStatus::SUCCESS; + + if(get_mode_cli_->service_is_ready()){ + auto request = std::make_shared(); + auto get_mode_future_ = get_mode_cli_->async_send_request(request); + + if (get_mode_future_.wait_for(1s) == std::future_status::ready) + { + auto get_mode_result_ = get_mode_future_.get(); + if(get_mode_result_->current_mode == mode_name){ + return BT::NodeStatus::SUCCESS; + } + + if(change_mode_cli_->service_is_ready()){ + auto request = std::make_shared(); + request->mode_name = mode_name; + auto change_mode_future_ = change_mode_cli_->async_send_request(request); + + if (change_mode_future_.wait_for(1s) == std::future_status::ready) + { + auto change_mode_result_ = change_mode_future_.get(); + if(!change_mode_result_->success){ + return BT::NodeStatus::FAILURE; + } + + if(change_mode_result_->success){ + RCLCPP_INFO(node_->get_logger(), "Node %s mode changed to %s", node_name_.c_str(), mode_name.c_str()); + previous_modes_->at(node_name_) = mode_name; + return BT::NodeStatus::SUCCESS; + } + } else { + return BT::NodeStatus::FAILURE; + } + } + + RCLCPP_INFO(node_->get_logger(), "%s/change_mode service not available...", node_name_.c_str()); + return BT::NodeStatus::FAILURE; + } else { + return BT::NodeStatus::FAILURE; + } + } + + RCLCPP_INFO(node_->get_logger(), "%s/get_mode service not available...", node_name_.c_str()); + return BT::NodeStatus::FAILURE; + + } +} //namespace suave_bt diff --git a/suave_managing/suave_bt/src/suave_bt/action_inspect_pipeline.cpp b/suave_managing/suave_bt/src/suave_bt/action_inspect_pipeline.cpp new file mode 100644 index 0000000..f367880 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/action_inspect_pipeline.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/action_inspect_pipeline.hpp" + +namespace suave_bt +{ + InspectPipeline::InspectPipeline( + const std::string& name, const BT::NodeConfig & conf) + : BT::StatefulActionNode(name, conf), pipeline_inspected_(false) + { + node_ = config().blackboard->get>("node"); + pipeline_inspected_sub_ = node_->create_subscription( + "/pipeline/inspected", + 10, + std::bind(&InspectPipeline::pipeline_inspected_cb, this, _1)); + } + + void InspectPipeline::pipeline_inspected_cb(const std_msgs::msg::Bool &msg){ + pipeline_inspected_ = msg.data; + } + + BT::NodeStatus InspectPipeline::onRunning(){ + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + if(this->node_->time_limit_reached()){ + std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; + return BT::NodeStatus::FAILURE; + } + + if(pipeline_inspected_==true){ + std::cout << "Async action finished: "<< this->name() << std::endl; + return BT::NodeStatus::SUCCESS; + } + std::cout<<"Inspecting pipeline! "<get>("node"); + battery_level_sub_ = this->node_->create_subscription( + "/battery_monitor/recharge/complete", + 10, + std::bind(&RechargeBattery::battery_level_cb, this, _1)); + } + + void RechargeBattery::battery_level_cb(const std_msgs::msg::Bool &msg){ + recharged_ = msg.data; + } + + BT::NodeStatus RechargeBattery::onRunning() { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + if(this->node_->time_limit_reached()){ + std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; + return BT::NodeStatus::FAILURE; + } + + if(recharged_ == true){ + std::cout << "Async action finished: "<< this->name() << std::endl; + return BT::NodeStatus::SUCCESS; + } + std::cout<<"Recharging! "<get>("node"); + pipeline_detection_sub_ = node_->create_subscription( + "/pipeline/detected", + 10, + std::bind(&SearchPipeline::pipeline_detected_cb, this, _1)); + } + + void SearchPipeline::pipeline_detected_cb(const std_msgs::msg::Bool &msg) + { + pipeline_detected_ = msg.data; + } + + BT::NodeStatus SearchPipeline::onStart() + { + //request node activation + this->node_->set_search_started(); + return BT::NodeStatus::RUNNING; + } + + BT::NodeStatus SearchPipeline::onRunning() + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + if(node_->time_limit_reached()){ + std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; + return BT::NodeStatus::FAILURE; + } + + if(pipeline_detected_ == true){ + std::cout << "Async action finished: "<< this->name() << std::endl; + return BT::NodeStatus::SUCCESS; + } + std::cout<<"Searching for pipeline! "<get>("node"); + set_guided_cli_ = _node->create_client( + "mavros/set_mode"); + mavros_state_sub_ = _node->create_subscription( + "mavros/state", + 10, + std::bind(&SetGuidedMode::state_cb, this, _1)); + } + + void + SetGuidedMode::state_cb(const mavros_msgs::msg::State &msg) + { + mode_ = msg.mode; + } + + + BT::NodeStatus SetGuidedMode::onRunning(){ + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + if(set_guided_cli_->service_is_ready()){ + auto request = std::make_shared(); + request->custom_mode = "GUIDED"; + auto set_guided_result_ = set_guided_cli_->async_send_request(request); + + if (set_guided_result_.wait_for(1s) == std::future_status::ready) + { + auto result_ = set_guided_result_.get(); + if(!result_->mode_sent){ + return BT::NodeStatus::FAILURE; + } + + if(result_->mode_sent && mode_ == "GUIDED"){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Mode set to GUIDED!"); + return BT::NodeStatus::SUCCESS; + } + } else { + return BT::NodeStatus::FAILURE; + } + } + + return BT::NodeStatus::RUNNING; + } +} //namespace suave_bt diff --git a/suave_managing/suave_bt/src/suave_bt/condition_battery_level.cpp b/suave_managing/suave_bt/src/suave_bt/condition_battery_level.cpp new file mode 100644 index 0000000..342f67e --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/condition_battery_level.cpp @@ -0,0 +1,52 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/condition_battery_level.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + BatteryLevel::BatteryLevel( + const std::string & xml_tag_name, + const BT::NodeConfig & conf) + : BT::ConditionNode(xml_tag_name, conf), battery_level_(-1.0) + { + node_ = config().blackboard->get>("node"); + diagnostics_sub_ = node_->create_subscription( + "/diagnostics", + 10, + std::bind(&BatteryLevel::diagnostics_cb, this, _1)); + } + + void BatteryLevel::diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg){ + for(auto status: msg.status){ + if(status.message == "QA status"){ + for(auto value: status.values){ + if(value.key == "battery_level") { + battery_level_ = std::stof(value.value); + } + } + } + } + } + + BT::NodeStatus BatteryLevel::tick() { + float threshold; + getInput("threshold", threshold); + if(battery_level_ >= threshold || battery_level_ == -1.0) return BT::NodeStatus::SUCCESS; + + return BT::NodeStatus::FAILURE; + } +} diff --git a/suave_managing/suave_bt/src/suave_bt/condition_thrusters_ok.cpp b/suave_managing/suave_bt/src/suave_bt/condition_thrusters_ok.cpp new file mode 100644 index 0000000..d9c9af0 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/condition_thrusters_ok.cpp @@ -0,0 +1,62 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/condition_thrusters_ok.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + ThrustersOk::ThrustersOk( + const std::string & xml_tag_name, + const BT::NodeConfig & conf) + : BT::ConditionNode(xml_tag_name, conf) + { + thrusters_ok_["c_thruster_1"] = true; + thrusters_ok_["c_thruster_2"] = true; + thrusters_ok_["c_thruster_3"] = true; + thrusters_ok_["c_thruster_4"] = true; + thrusters_ok_["c_thruster_5"] = true; + thrusters_ok_["c_thruster_6"] = true; + + node_ = config().blackboard->get>("node"); + diagnostics_sub_ = node_->create_subscription( + "/diagnostics", + 10, + std::bind(&ThrustersOk::diagnostics_cb, this, _1)); + } + + void ThrustersOk::diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg){ + for(auto status: msg.status){ + if(status.message == "Component status"){ + for(auto value: status.values){ + auto it = thrusters_ok_.find(value.key); + if (it != thrusters_ok_.end()) { + it->second = (value.value == "RECOVERED") ? true : false; + } + } + } + } + } + + + BT::NodeStatus ThrustersOk::tick() { + // check if whole thrusters_ok_ map is true + if(std::all_of(thrusters_ok_.begin(), thrusters_ok_.end(), [](const auto& t) { return t.second; })){ + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; + } + +} diff --git a/suave_managing/suave_bt/src/suave_bt/condition_water_visibility.cpp b/suave_managing/suave_bt/src/suave_bt/condition_water_visibility.cpp new file mode 100644 index 0000000..1e1e3bb --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/condition_water_visibility.cpp @@ -0,0 +1,51 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/condition_water_visibility.hpp" + +using namespace std::placeholders; + +namespace suave_bt +{ + WaterVisibility::WaterVisibility( + const std::string & xml_tag_name, + const BT::NodeConfig & conf) + : BT::ConditionNode(xml_tag_name, conf), water_visibility_(-1.0) + { + node_ = config().blackboard->get>("node"); + diagnostics_sub_ = node_->create_subscription( + "/diagnostics", + 10, + std::bind(&WaterVisibility::diagnostics_cb, this, _1)); + } + + void WaterVisibility::diagnostics_cb(const diagnostic_msgs::msg::DiagnosticArray &msg){ + for(auto status: msg.status){ + if(status.message == "QA status"){ + for(auto value: status.values){ + if(value.key == "water_visibility") { + water_visibility_ = std::stof(value.value); + } + } + } + } + } + + BT::NodeStatus WaterVisibility::tick() { + float threshold; + getInput("threshold", threshold); + if(water_visibility_ >= threshold || water_visibility_ == -1.0) return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; + } +} diff --git a/suave_managing/suave_bt/src/suave_bt/is_pipeline_found.cpp b/suave_managing/suave_bt/src/suave_bt/is_pipeline_found.cpp new file mode 100644 index 0000000..9c7c1c5 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/is_pipeline_found.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/is_pipeline_found.hpp" + +namespace suave_bt +{ + +using namespace std::placeholders; + +IsPipelineFound::IsPipelineFound( + const std::string & xml_tag_name, + const BT::NodeConfig & conf) +: BT::ConditionNode(xml_tag_name, conf), _pipeline_detected(false) +{ + _node = config().blackboard->get>("node"); + + pipeline_detection_sub_ = _node->create_subscription( + "/pipeline/detected", + 10, + std::bind(&IsPipelineFound::pipeline_detected_cb, this, _1)); +} + +void +IsPipelineFound::pipeline_detected_cb(const std_msgs::msg::Bool &msg) +{ + _pipeline_detected = msg.data; +} + +BT::NodeStatus +IsPipelineFound::tick() +{ + return (_pipeline_detected==true) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +} // namespace suave_bt diff --git a/suave_managing/suave_bt/src/suave_bt/is_pipeline_inspected.cpp b/suave_managing/suave_bt/src/suave_bt/is_pipeline_inspected.cpp new file mode 100644 index 0000000..7f52a72 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/is_pipeline_inspected.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/is_pipeline_inspected.hpp" + +namespace suave_bt +{ + +using namespace std::placeholders; + +IsPipelineInspected::IsPipelineInspected( + const std::string & xml_tag_name, + const BT::NodeConfig & conf) +: BT::ConditionNode(xml_tag_name, conf), _pipeline_inspected(false) +{ + _node = config().blackboard->get>("node"); + + pipeline_inspected_sub_ = _node->create_subscription( + "/pipeline/inspected", + 10, + std::bind(&IsPipelineInspected::pipeline_inspected_cb, this, _1)); +} + +void +IsPipelineInspected::pipeline_inspected_cb(const std_msgs::msg::Bool &msg) +{ + _pipeline_inspected = msg.data; +} + +BT::NodeStatus +IsPipelineInspected::tick() +{ + return (_pipeline_inspected==true) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +} // namespace suave_bt diff --git a/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp b/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp new file mode 100644 index 0000000..3e28356 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt/suave_mission.cpp @@ -0,0 +1,83 @@ +// Copyright 2023 Gustavo Rezende Silva +// +// 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 "suave_bt/suave_mission.hpp" + +namespace suave_bt +{ + using namespace std::placeholders; + using namespace std::chrono_literals; + + SuaveMission::SuaveMission(std::string none_name) + : Node(none_name), search_started_(false), mission_aborted_(false) + { + this->declare_parameter("time_limit", 300); + time_limit_ = this->get_parameter("time_limit").as_int(); + + save_mission_results_cli = + this->create_client("mission_metrics/save"); + + time_limit_timer_cb_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + // TODO: create parameter for timer rate? + time_limit_timer_ = this->create_wall_timer( + 100ms, std::bind(&SuaveMission::time_limit_cb, this), time_limit_timer_cb_group_); + } + + void SuaveMission::time_limit_cb(){ + if(this->time_limit_reached()){ + RCLCPP_INFO(this->get_logger(), "Time limit reached!"); + this->finish_mission(); + } + } + + bool SuaveMission::time_limit_reached(){ + if(search_started_){ + return (this->get_clock()->now() - start_time_) >= rclcpp::Duration(time_limit_, 0); + } + return false; + } + + void SuaveMission::finish_mission(){ + this->request_save_mission_results(); + this->time_limit_timer_->cancel(); + mission_aborted_ = true; + } + + bool SuaveMission::request_save_mission_results(){ + while (!save_mission_results_cli->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return false; + } + RCLCPP_INFO(this->get_logger(), "mission_metrics/save service not available, waiting again..."); + } + + auto request = std::make_shared(); + auto response = save_mission_results_cli->async_send_request(request); + if (response.wait_for(1s) == std::future_status::ready) + { + return true; + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to call service mission_metrics/save"); + return false; + } + } + + void SuaveMission::set_search_started(){ + start_time_ = this->get_clock()->now(); + search_started_ = true; + } + +} diff --git a/suave_managing/suave_bt/src/suave_bt_extended.cpp b/suave_managing/suave_bt/src/suave_bt_extended.cpp new file mode 100644 index 0000000..560ab48 --- /dev/null +++ b/suave_managing/suave_bt/src/suave_bt_extended.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 Gustavo Rezende Silva +// +// 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 "ament_index_cpp/get_package_share_directory.hpp" + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" + +#include "behaviortree_cpp/loggers/groot2_publisher.h" +#include "behaviortree_cpp/xml_parsing.h" +#include "behaviortree_cpp/json_export.h" + +#include "rclcpp/rclcpp.hpp" + +#include "suave_bt/suave_mission.hpp" +#include "suave_bt/is_pipeline_found.hpp" +#include "suave_bt/is_pipeline_inspected.hpp" +#include "suave_bt/condition_water_visibility.hpp" +#include "suave_bt/condition_thrusters_ok.hpp" +#include "suave_bt/condition_battery_level.hpp" + +#include "suave_bt/action_arm_thrusters.hpp" +#include "suave_bt/action_set_guided_mode.hpp" +#include "suave_bt/action_search_pipeline.hpp" +#include "suave_bt/action_recharge_battery.hpp" +#include "suave_bt/action_inspect_pipeline.hpp" +#include "suave_bt/action_change_mode.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = std::make_shared("mission_node"); + + BT::BehaviorTreeFactory factory; + BT::SharedLibrary loader; + + factory.registerNodeType("search_pipeline"); + factory.registerNodeType("inspect_pipeline"); + factory.registerNodeType("recharge"); + + factory.registerNodeType("arm_thrusters"); + factory.registerNodeType("set_guided_mode"); + factory.registerNodeType("change_mode"); + + factory.registerNodeType("water_visibility"); + factory.registerNodeType("thrusters_ok"); + factory.registerNodeType("battery_level"); + + factory.registerNodeType("is_pipeline_found"); + factory.registerNodeType("is_pipeline_inspected"); + + factory.registerSimpleCondition("is_mission_aborted", [&](const auto&) { return node->is_mission_aborted() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; }); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("suave_bt"); + std::string xml_file = pkgpath + "/bts/suave_extended.xml"; + + std::map previous_modes; + previous_modes["f_generate_search_path"] = ""; + previous_modes["f_follow_pipeline"] = ""; + previous_modes["f_maintain_motion"] = ""; + previous_modes["generate_recharge_path"] = ""; + + auto blackboard = BT::Blackboard::create(); + blackboard->set>("node", node); + blackboard->set>>( + "previous_modes", + std::make_shared>(previous_modes)); + + BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); + // Connect the Groot2Publisher. This will allow Groot2 to + // get the tree and poll status updates. + const unsigned port = 1667; + BT::Groot2Publisher publisher(tree, port); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + std::thread t([&executor]() { + executor.spin(); + }); + + bool finish = false; + while (!finish & rclcpp::ok()) { + finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + std::this_thread::sleep_for(100ms); + } + + if(!node->is_mission_aborted()) node->finish_mission(); + std::this_thread::sleep_for(1s); + + executor.cancel(); + t.join(); + rclcpp::shutdown(); + return 0; +}