diff --git a/suave_rosa_bt/bts/suave.xml b/suave_rosa_bt/bts/suave.xml index 22954e6..82ff8a2 100644 --- a/suave_rosa_bt/bts/suave.xml +++ b/suave_rosa_bt/bts/suave.xml @@ -3,59 +3,44 @@ main_tree_to_execute="MainTree"> - + + - - + + + + + + + + + + + + + + - - - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - + - + + - - - diff --git a/suave_rosa_bt/bts/suave_extended.xml b/suave_rosa_bt/bts/suave_extended.xml index 3473b2a..9589300 100644 --- a/suave_rosa_bt/bts/suave_extended.xml +++ b/suave_rosa_bt/bts/suave_extended.xml @@ -3,79 +3,65 @@ main_tree_to_execute="MainTree"> - + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - + - + diff --git a/suave_rosa_bt/include/suave_rosa_bt/action_inspect_pipeline.hpp b/suave_rosa_bt/include/suave_rosa_bt/action_inspect_pipeline.hpp index 7fe8365..b1d8ad2 100644 --- a/suave_rosa_bt/include/suave_rosa_bt/action_inspect_pipeline.hpp +++ b/suave_rosa_bt/include/suave_rosa_bt/action_inspect_pipeline.hpp @@ -34,9 +34,9 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction{ public: InspectPipeline( const std::string& name, const BT::NodeConfig & conf) - : rosa_task_plan_bt::RosaAction(name, conf), _pipeline_inspected(false) + : rosa_task_plan_bt::RosaAction(name, conf), pipeline_inspected_(false) { - pipeline_inspected_sub_ = this->_node->template create_subscription( + pipeline_inspected_sub_ = this->node_->template create_subscription( "/pipeline/inspected", 10, std::bind(&InspectPipeline::pipeline_inspected_cb, this, _1)); @@ -45,13 +45,13 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction{ BT::NodeStatus onRunning() override { std::this_thread::sleep_for(std::chrono::milliseconds(50)); - if(this->_node->template time_limit_reached()){ + if(this->node_->template time_limit_reached()){ std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::FAILURE; } - if(_pipeline_inspected==true){ + if(pipeline_inspected_==true){ std::cout << "Async action finished: "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::SUCCESS; @@ -68,10 +68,10 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction{ } private: - bool _pipeline_inspected; + bool pipeline_inspected_; rclcpp::Subscription::SharedPtr pipeline_inspected_sub_; void pipeline_inspected_cb(const std_msgs::msg::Bool &msg){ - _pipeline_inspected = msg.data; + pipeline_inspected_ = msg.data; }; }; diff --git a/suave_rosa_bt/include/suave_rosa_bt/action_recharge_battery.hpp b/suave_rosa_bt/include/suave_rosa_bt/action_recharge_battery.hpp index efcc196..4e51a71 100644 --- a/suave_rosa_bt/include/suave_rosa_bt/action_recharge_battery.hpp +++ b/suave_rosa_bt/include/suave_rosa_bt/action_recharge_battery.hpp @@ -36,7 +36,7 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction{ const std::string& name, const BT::NodeConfig & conf) : rosa_task_plan_bt::RosaAction(name, conf) { - battery_level_sub = this->_node->template create_subscription( + battery_level_sub_ = this->node_->template create_subscription( "/battery_monitor/recharge/complete", 10, std::bind(&RechargeBattery::battery_level_cb, this, _1)); @@ -45,20 +45,20 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction{ BT::NodeStatus onStart(){ std::cout << "Async action starting: " << this->name() << std::endl; // _completion_time = std::chrono::system_clock::now() + std::chrono::milliseconds(5000); - _recharged = false; + recharged_ = false; return rosa_task_plan_bt::RosaAction::onStart(); }; BT::NodeStatus onRunning() override { std::this_thread::sleep_for(std::chrono::milliseconds(50)); - if(this->_node->template time_limit_reached()){ + if(this->node_->template time_limit_reached()){ std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::FAILURE; } - if(_recharged == true){ + if(recharged_ == true){ std::cout << "Async action finished: "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::SUCCESS; @@ -76,10 +76,10 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction{ private: // std::chrono::system_clock::time_point _completion_time; - bool _recharged = false; - rclcpp::Subscription::SharedPtr battery_level_sub; + bool recharged_ = false; + rclcpp::Subscription::SharedPtr battery_level_sub_; void battery_level_cb(const std_msgs::msg::Bool &msg){ - _recharged = msg.data; + recharged_ = msg.data; }; }; diff --git a/suave_rosa_bt/include/suave_rosa_bt/action_search_pipeline.hpp b/suave_rosa_bt/include/suave_rosa_bt/action_search_pipeline.hpp index 7d2bdde..6fa9bff 100644 --- a/suave_rosa_bt/include/suave_rosa_bt/action_search_pipeline.hpp +++ b/suave_rosa_bt/include/suave_rosa_bt/action_search_pipeline.hpp @@ -34,29 +34,29 @@ class SearchPipeline : public rosa_task_plan_bt::RosaAction{ public: SearchPipeline( const std::string& name, const BT::NodeConfig & conf) - : rosa_task_plan_bt::RosaAction(name, conf), _pipeline_detected(false) + : rosa_task_plan_bt::RosaAction(name, conf), pipeline_detected_(false) { - pipeline_detection_sub_ = this->_node->template create_subscription( + pipeline_detection_sub_ = this->node_->template create_subscription( "/pipeline/detected", 10, std::bind(&SearchPipeline::pipeline_detected_cb, this, _1)); }; BT::NodeStatus onStart(){ - this->_node->template set_search_started(); + this->node_->template set_search_started(); return rosa_task_plan_bt::RosaAction::onStart(); }; BT::NodeStatus onRunning() override{ std::this_thread::sleep_for(std::chrono::milliseconds(50)); - if(this->_node->template time_limit_reached()){ + if(this->node_->template time_limit_reached()){ std::cout << "Time limit reached. Canceling action "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::FAILURE; } - if(_pipeline_detected == true){ + if(pipeline_detected_ == true){ std::cout << "Async action finished: "<< this->name() << std::endl; this->cancel_action(); return BT::NodeStatus::SUCCESS; @@ -73,11 +73,11 @@ class SearchPipeline : public rosa_task_plan_bt::RosaAction{ } protected: - bool _pipeline_detected; + bool pipeline_detected_; rclcpp::Subscription::SharedPtr pipeline_detection_sub_; void pipeline_detected_cb(const std_msgs::msg::Bool &msg){ - _pipeline_detected = msg.data; + pipeline_detected_ = msg.data; }; }; diff --git a/suave_rosa_bt/include/suave_rosa_bt/arm_thrusters.hpp b/suave_rosa_bt/include/suave_rosa_bt/arm_thrusters.hpp deleted file mode 100644 index ba6dc1e..0000000 --- a/suave_rosa_bt/include/suave_rosa_bt/arm_thrusters.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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_ROSA_BT__ARM_THRUSTERS_HPP_ -#define SUAVE_ROSA_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_rosa_bt/suave_mission.hpp" - - -namespace suave_rosa_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_rosa_bt::SuaveMission::SharedPtr _node; - rclcpp::Client::SharedPtr arm_motors_cli_; -}; - -} // namespace suave_rosa_bt - -#endif // SUAVE_ROSA_BT__ARM_THRUSTERS_HPP_ diff --git a/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_found.hpp b/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_found.hpp deleted file mode 100644 index e02a10f..0000000 --- a/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_found.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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_ROSA_BT__PIPELINE_FOUND_HPP_ -#define SUAVE_ROSA_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_rosa_bt/suave_mission.hpp" - -namespace suave_rosa_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_rosa_bt::SuaveMission::SharedPtr _node; - rclcpp::Subscription::SharedPtr pipeline_detection_sub_; - - void pipeline_detected_cb(const std_msgs::msg::Bool &msg); -}; - -} // namespace suave_rosa_bt - -#endif // SUAVE_ROSA_BT__PIPELINE_FOUND_HPP_ diff --git a/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_inspected.hpp b/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_inspected.hpp deleted file mode 100644 index eae6bb5..0000000 --- a/suave_rosa_bt/include/suave_rosa_bt/is_pipeline_inspected.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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_ROSA_BT__PIPELINE_INSPECTED_HPP_ -#define SUAVE_ROSA_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_rosa_bt/suave_mission.hpp" - -namespace suave_rosa_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_rosa_bt::SuaveMission::SharedPtr _node; - rclcpp::Subscription::SharedPtr pipeline_inspected_sub_; - - void pipeline_inspected_cb(const std_msgs::msg::Bool &msg); -}; - -} // namespace suave_rosa_bt - -#endif // SUAVE_ROSA_BT__PIPELINE_INSPECTED_HPP_ diff --git a/suave_rosa_bt/include/suave_rosa_bt/set_guided_mode.hpp b/suave_rosa_bt/include/suave_rosa_bt/set_guided_mode.hpp deleted file mode 100644 index 87a0030..0000000 --- a/suave_rosa_bt/include/suave_rosa_bt/set_guided_mode.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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_ROSA_BT__SET_GUIDED_MODE_HPP_ -#define SUAVE_ROSA_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_rosa_bt/suave_mission.hpp" - -namespace suave_rosa_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_rosa_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_rosa_bt - -#endif // SUAVE_ROSA_BT__SET_GUIDED_MODE_HPP_ diff --git a/suave_rosa_bt/include/suave_rosa_bt/suave_mission.hpp b/suave_rosa_bt/include/suave_rosa_bt/suave_mission.hpp deleted file mode 100644 index eb42088..0000000 --- a/suave_rosa_bt/include/suave_rosa_bt/suave_mission.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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_ROSA_BT__SUAVE_MISSION_HPP_ -#define SUAVE_ROSA_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_rosa_bt -{ - -class SuaveMission : public rclcpp::Node{ - -public: - SuaveMission(std::string none_name); - - bool time_limit_reached(); - - bool request_save_mission_results(); - void set_search_started(); - -private: - rclcpp::Time _start_time; - bool _search_started = false; - int _time_limit; - rclcpp::Client::SharedPtr save_mission_results_cli; -}; - -} // namespace suave_rosa_bt - -#endif // SUAVE_ROSA_BT__SUAVE_MISSION_HPP_ diff --git a/suave_rosa_bt/include/suave_rosa_bt/visibility_control.h b/suave_rosa_bt/include/suave_rosa_bt/visibility_control.h new file mode 100644 index 0000000..2925e9d --- /dev/null +++ b/suave_rosa_bt/include/suave_rosa_bt/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef SUAVE_ROSA_BT__VISIBILITY_CONTROL_H_ +#define SUAVE_ROSA_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_ROSA_BT_EXPORT __attribute__ ((dllexport)) + #define SUAVE_ROSA_BT_IMPORT __attribute__ ((dllimport)) + #else + #define SUAVE_ROSA_BT_EXPORT __declspec(dllexport) + #define SUAVE_ROSA_BT_IMPORT __declspec(dllimport) + #endif + #ifdef SUAVE_ROSA_BT_BUILDING_LIBRARY + #define SUAVE_ROSA_BT_PUBLIC SUAVE_ROSA_BT_EXPORT + #else + #define SUAVE_ROSA_BT_PUBLIC SUAVE_ROSA_BT_IMPORT + #endif + #define SUAVE_ROSA_BT_PUBLIC_TYPE SUAVE_ROSA_BT_PUBLIC + #define SUAVE_ROSA_BT_LOCAL +#else + #define SUAVE_ROSA_BT_EXPORT __attribute__ ((visibility("default"))) + #define SUAVE_ROSA_BT_IMPORT + #if __GNUC__ >= 4 + #define SUAVE_ROSA_BT_PUBLIC __attribute__ ((visibility("default"))) + #define SUAVE_ROSA_BT_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define SUAVE_ROSA_BT_PUBLIC + #define SUAVE_ROSA_BT_LOCAL + #endif + #define SUAVE_ROSA_BT_PUBLIC_TYPE +#endif + +#endif // SUAVE_ROSA_BT__VISIBILITY_CONTROL_H_ diff --git a/suave_rosa_bt/launch/suave_rosa_bt.launch.py b/suave_rosa_bt/launch/suave_rosa_bt.launch.py index dd346a2..49ac80b 100644 --- a/suave_rosa_bt/launch/suave_rosa_bt.launch.py +++ b/suave_rosa_bt/launch/suave_rosa_bt.launch.py @@ -29,9 +29,8 @@ def generate_launch_description(): mission_type_arg = DeclareLaunchArgument( 'mission_type', - default_value='time_constrained_mission', - description='Desired mission type' + - '[time_constrained_mission or const_dist_mission]' + default_value='suave', + description='Mission name for logging' ) result_filename_arg = DeclareLaunchArgument( diff --git a/suave_rosa_bt/launch/suave_rosa_extended_bt.launch.py b/suave_rosa_bt/launch/suave_rosa_extended_bt.launch.py index 1c7ee1d..9ed30b1 100644 --- a/suave_rosa_bt/launch/suave_rosa_extended_bt.launch.py +++ b/suave_rosa_bt/launch/suave_rosa_extended_bt.launch.py @@ -29,9 +29,8 @@ def generate_launch_description(): mission_type_arg = DeclareLaunchArgument( 'mission_type', - default_value='time_constrained_mission', - description='Desired mission type' + - '[time_constrained_mission or const_dist_mission]' + default_value='suave_extended', + description='Mission name for logging' ) result_filename_arg = DeclareLaunchArgument( diff --git a/suave_rosa_bt/src/suave_rosa_bt.cpp b/suave_rosa_bt/src/suave_rosa_bt.cpp index 66723f8..c4d2e19 100644 --- a/suave_rosa_bt/src/suave_rosa_bt.cpp +++ b/suave_rosa_bt/src/suave_rosa_bt.cpp @@ -18,57 +18,76 @@ #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/action_arm_thrusters.hpp" +#include "suave_bt/action_set_guided_mode.hpp" +#include "suave_bt/is_pipeline_found.hpp" +#include "suave_bt/is_pipeline_inspected.hpp" + +#include "suave_bt/suave_mission.hpp" + #include "suave_rosa_bt/action_recharge_battery.hpp" #include "suave_rosa_bt/action_search_pipeline.hpp" #include "suave_rosa_bt/action_inspect_pipeline.hpp" -#include "suave_rosa_bt/arm_thrusters.hpp" -#include "suave_rosa_bt/set_guided_mode.hpp" -#include "suave_rosa_bt/suave_mission.hpp" #include "rosa_task_plan_bt/is_action_feasible.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - std::shared_ptr node = std::make_shared("mission_node"); + 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>>("search_pipeline"); + factory.registerNodeType>>("inspect_pipeline"); + factory.registerNodeType>>("recharge"); + + factory.registerNodeType>>("is_action_feasible"); - factory.registerNodeType>>("IsActionFeasible"); + factory.registerNodeType("arm_thrusters"); + factory.registerNodeType("set_guided_mode"); + + factory.registerSimpleCondition( + "is_mission_aborted", + [&](const auto&) { return node->is_mission_aborted() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; }); - factory.registerNodeType("ArmThrusters"); - factory.registerNodeType("SetGuidedMode"); std::string pkgpath = ament_index_cpp::get_package_share_directory("suave_rosa_bt"); std::string xml_file = pkgpath + "/bts/suave.xml"; auto blackboard = BT::Blackboard::create(); - blackboard->set>("node", node); + blackboard->set>("node", node); BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); - rclcpp::Rate rate(10); + 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()) { - if(node->time_limit_reached()){ - node->request_save_mission_results(); - tree.haltTree(); - finish = true; - } else{ - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; - } - rclcpp::spin_some(node); - rate.sleep(); + 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_rosa_bt/src/suave_rosa_bt/arm_thrusters.cpp b/suave_rosa_bt/src/suave_rosa_bt/arm_thrusters.cpp deleted file mode 100644 index 41491dd..0000000 --- a/suave_rosa_bt/src/suave_rosa_bt/arm_thrusters.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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_rosa_bt/arm_thrusters.hpp" - - -namespace suave_rosa_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); - - // Wait for the result. - if (rclcpp::spin_until_future_complete(_node, arm_motors_result_, std::chrono::milliseconds(100)) == - rclcpp::FutureReturnCode::SUCCESS) - { - auto arm_result_ = arm_motors_result_.get(); - if(!arm_result_->success){ - return BT::NodeStatus::FAILURE; - } - - if(arm_result_->success){ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Thrusters armed!"); - return BT::NodeStatus::SUCCESS; - } - } else { - return BT::NodeStatus::FAILURE; - } - } - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "mavros/cmd/arming service not available, waiting again..."); - return BT::NodeStatus::RUNNING; - } -} //namespace suave_rosa_bt diff --git a/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_found.cpp b/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_found.cpp deleted file mode 100644 index 3155280..0000000 --- a/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_found.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// 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_rosa_bt/is_pipeline_found.hpp" - -namespace suave_rosa_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_rosa_bt diff --git a/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_inspected.cpp b/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_inspected.cpp deleted file mode 100644 index 944f935..0000000 --- a/suave_rosa_bt/src/suave_rosa_bt/is_pipeline_inspected.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// 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_rosa_bt/is_pipeline_inspected.hpp" - -namespace suave_rosa_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_rosa_bt diff --git a/suave_rosa_bt/src/suave_rosa_bt/set_guided_mode.cpp b/suave_rosa_bt/src/suave_rosa_bt/set_guided_mode.cpp deleted file mode 100644 index aa26ab9..0000000 --- a/suave_rosa_bt/src/suave_rosa_bt/set_guided_mode.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// 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_rosa_bt/set_guided_mode.hpp" - - -namespace suave_rosa_bt -{ - using namespace std::placeholders; - using namespace std::chrono_literals; - - SetGuidedMode::SetGuidedMode( - const std::string& name, const BT::NodeConfig & conf) - : BT::StatefulActionNode(name, conf) - { - _node = config().blackboard->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); - - // Wait for the result. - if (rclcpp::spin_until_future_complete(_node, set_guided_result_, std::chrono::milliseconds(1000)) == - rclcpp::FutureReturnCode::SUCCESS) - { - 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_rosa_bt diff --git a/suave_rosa_bt/src/suave_rosa_bt/suave_mission.cpp b/suave_rosa_bt/src/suave_rosa_bt/suave_mission.cpp deleted file mode 100644 index 7a97156..0000000 --- a/suave_rosa_bt/src/suave_rosa_bt/suave_mission.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// 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_rosa_bt/suave_mission.hpp" - -namespace suave_rosa_bt -{ - using namespace std::placeholders; - using namespace std::chrono_literals; - - SuaveMission::SuaveMission(std::string none_name): Node(none_name){ - 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"); - } - - - bool SuaveMission::time_limit_reached(){ - if(_search_started){ - std::cout << "Time limit reached!"<< std::endl; - return (this->get_clock()->now() - _start_time) >= rclcpp::Duration(_time_limit, 0); - } - return false; - } - - 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 (rclcpp::spin_until_future_complete(this->get_node_base_interface(), response) == - rclcpp::FutureReturnCode::SUCCESS) - { - 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_rosa_bt/src/suave_rosa_bt_extended.cpp b/suave_rosa_bt/src/suave_rosa_bt_extended.cpp index dc0ab32..db000e2 100644 --- a/suave_rosa_bt/src/suave_rosa_bt_extended.cpp +++ b/suave_rosa_bt/src/suave_rosa_bt_extended.cpp @@ -18,61 +18,76 @@ #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/action_arm_thrusters.hpp" +#include "suave_bt/action_set_guided_mode.hpp" +#include "suave_bt/suave_mission.hpp" +#include "suave_bt/is_pipeline_found.hpp" +#include "suave_bt/is_pipeline_inspected.hpp" + #include "suave_rosa_bt/action_recharge_battery.hpp" #include "suave_rosa_bt/action_search_pipeline.hpp" #include "suave_rosa_bt/action_inspect_pipeline.hpp" -#include "suave_rosa_bt/arm_thrusters.hpp" -#include "suave_rosa_bt/set_guided_mode.hpp" -#include "suave_rosa_bt/suave_mission.hpp" -#include "suave_rosa_bt/is_pipeline_found.hpp" -#include "suave_rosa_bt/is_pipeline_inspected.hpp" #include "rosa_task_plan_bt/is_action_feasible.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - std::shared_ptr node = std::make_shared("mission_node"); + 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>>("search_pipeline"); + factory.registerNodeType>>("inspect_pipeline"); + factory.registerNodeType>>("recharge"); + + factory.registerNodeType>>("is_action_feasible"); + factory.registerNodeType("is_pipeline_found"); + factory.registerNodeType("is_pipeline_inspected"); - factory.registerNodeType>>("IsActionFeasible"); - factory.registerNodeType("IsPipelineFound"); - factory.registerNodeType("IsPipelineInspected"); + factory.registerNodeType("arm_thrusters"); + factory.registerNodeType("set_guided_mode"); - factory.registerNodeType("ArmThrusters"); - factory.registerNodeType("SetGuidedMode"); + 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_rosa_bt"); std::string xml_file = pkgpath + "/bts/suave_extended.xml"; auto blackboard = BT::Blackboard::create(); - blackboard->set>("node", node); + blackboard->set>("node", node); BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); - rclcpp::Rate rate(10); + 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()) { - if(node->time_limit_reached()){ - node->request_save_mission_results(); - tree.haltTree(); - finish = true; - } else{ - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; - } - rclcpp::spin_some(node); - rate.sleep(); + 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; }