Skip to content

Commit

Permalink
♻️ refactor to adjust it to newest suave version
Browse files Browse the repository at this point in the history
  • Loading branch information
@Rezenders
Rezenders committed May 15, 2024
1 parent 8dbc585 commit f5f7d24
Show file tree
Hide file tree
Showing 20 changed files with 211 additions and 733 deletions.
67 changes: 26 additions & 41 deletions suave_rosa_bt/bts/suave.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,59 +3,44 @@
main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RetryUntilSuccessful num_attempts="-1">
<SequenceWithMemory>
<Fallback>
<is_mission_aborted/>
<SequenceWithMemory>
<ArmThrusters/>
<SetGuidedMode/>
<SequenceWithMemory>
<arm_thrusters/>
<set_guided_mode/>
</SequenceWithMemory>
<SequenceWithMemory>
<ReactiveSequence>
<is_action_feasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
<ReactiveSequence>
<is_action_feasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</SequenceWithMemory>
</SequenceWithMemory>
<SequenceWithMemory>
<ReactiveSequence>
<IsActionFeasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
<ReactiveSequence>
<IsActionFeasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</SequenceWithMemory>
</SequenceWithMemory>
</Fallback>
</RetryUntilSuccessful>
</BehaviorTree>

<BehaviorTree ID="inspect_tree">
<ReactiveSequence>
<IsActionFeasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</BehaviorTree>

<BehaviorTree ID="recharge_tree">
<ReactiveSequence>
<IsActionFeasible action_name="recharge"/>
<recharge/>
</ReactiveSequence>
</BehaviorTree>

<BehaviorTree ID="search_tree">
<ReactiveSequence>
<IsActionFeasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
</BehaviorTree>

<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="ArmThrusters"
<Action ID="arm_thrusters"
editable="true"/>
<Condition ID="IsActionFeasible"
<Action ID="inspect_pipeline"
editable="true"/>
<Condition ID="is_action_feasible"
editable="true">
<input_port name="action_name"/>
</Condition>
<Action ID="SetGuidedMode"
<Condition ID="is_mission_aborted"
editable="true"/>
<Action ID="search_pipeline"
editable="true"/>
<Action ID="set_guided_mode"
editable="true"/>
<Action ID="inspect_pipeline"/>
<Action ID="recharge"/>
<Action ID="search_pipeline"/>
</TreeNodesModel>

</root>
102 changes: 44 additions & 58 deletions suave_rosa_bt/bts/suave_extended.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,79 +3,65 @@
main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RetryUntilSuccessful num_attempts="-1">
<SequenceWithMemory>
<Fallback>
<is_mission_aborted/>
<SequenceWithMemory>
<ArmThrusters/>
<SetGuidedMode/>
</SequenceWithMemory>
<ReactiveSequence>
<Fallback>
<SequenceWithMemory>
<Fallback>
<IsPipelineFound/>
<ReactiveSequence>
<IsActionFeasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
</Fallback>
<Fallback>
<IsPipelineInspected/>
<ReactiveSequence>
<IsActionFeasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</Fallback>
</SequenceWithMemory>
<SequenceWithMemory>
<arm_thrusters/>
<set_guided_mode/>
</SequenceWithMemory>
<ReactiveSequence>
<Fallback>
<SequenceWithMemory>
<Fallback>
<is_pipeline_found/>
<ReactiveSequence>
<is_action_feasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
</Fallback>
<Fallback>
<is_pipeline_inspected/>
<ReactiveSequence>
<is_action_feasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</Fallback>
</SequenceWithMemory>
<ReactiveSequence>
<is_action_feasible action_name="recharge"/>
<recharge/>
</ReactiveSequence>
</Fallback>
<ReactiveSequence>
<IsActionFeasible action_name="recharge"/>
<recharge/>
<is_pipeline_found/>
<is_pipeline_inspected/>
</ReactiveSequence>
</Fallback>
<ReactiveSequence>
<IsPipelineFound/>
<IsPipelineInspected/>
</ReactiveSequence>
</ReactiveSequence>
</SequenceWithMemory>
</SequenceWithMemory>
</Fallback>
</RetryUntilSuccessful>
</BehaviorTree>

<BehaviorTree ID="inspect_tree">
<ReactiveSequence>
<IsActionFeasible action_name="inspect_pipeline"/>
<inspect_pipeline/>
</ReactiveSequence>
</BehaviorTree>

<BehaviorTree ID="recharge_tree">
<ReactiveSequence>
<IsActionFeasible action_name="recharge"/>
<recharge/>
</ReactiveSequence>
</BehaviorTree>

<BehaviorTree ID="search_tree">
<ReactiveSequence>
<IsActionFeasible action_name="search_pipeline"/>
<search_pipeline/>
</ReactiveSequence>
</BehaviorTree>

<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="ArmThrusters"
<Action ID="arm_thrusters"
editable="true"/>
<Condition ID="IsActionFeasible"
<Condition ID="is_action_feasible"
editable="true">
<input_port name="action_name"/>
</Condition>
<Condition ID="IsPipelineFound"/>
<Condition ID="IsPipelineInspected"/>
<Action ID="SetGuidedMode"
<Condition ID="is_pipeline_found"/>
<Condition ID="is_pipeline_inspected"/>
<Action ID="set_guided_mode"
editable="true"/>
<Action ID="inspect_pipeline"
editable="true"/>
<Action ID="inspect_pipeline"/>
<Condition ID="is_mission_aborted"
editable="true"/>
<Action ID="recharge"/>
<Action ID="search_pipeline"/>
<Action ID="search_pipeline"
editable="true"/>
</TreeNodesModel>

</root>
12 changes: 6 additions & 6 deletions suave_rosa_bt/include/suave_rosa_bt/action_inspect_pipeline.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction<T>{
public:
InspectPipeline(
const std::string& name, const BT::NodeConfig & conf)
: rosa_task_plan_bt::RosaAction<T>(name, conf), _pipeline_inspected(false)
: rosa_task_plan_bt::RosaAction<T>(name, conf), pipeline_inspected_(false)
{
pipeline_inspected_sub_ = this->_node->template create_subscription<std_msgs::msg::Bool>(
pipeline_inspected_sub_ = this->node_->template create_subscription<std_msgs::msg::Bool>(
"/pipeline/inspected",
10,
std::bind(&InspectPipeline::pipeline_inspected_cb, this, _1));
Expand All @@ -45,13 +45,13 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction<T>{
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;
Expand All @@ -68,10 +68,10 @@ class InspectPipeline : public rosa_task_plan_bt::RosaAction<T>{
}

private:
bool _pipeline_inspected;
bool pipeline_inspected_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pipeline_inspected_sub_;
void pipeline_inspected_cb(const std_msgs::msg::Bool &msg){
_pipeline_inspected = msg.data;
pipeline_inspected_ = msg.data;
};
};

Expand Down
14 changes: 7 additions & 7 deletions suave_rosa_bt/include/suave_rosa_bt/action_recharge_battery.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction<T>{
const std::string& name, const BT::NodeConfig & conf)
: rosa_task_plan_bt::RosaAction<T>(name, conf)
{
battery_level_sub = this->_node->template create_subscription<std_msgs::msg::Bool>(
battery_level_sub_ = this->node_->template create_subscription<std_msgs::msg::Bool>(
"/battery_monitor/recharge/complete",
10,
std::bind(&RechargeBattery::battery_level_cb, this, _1));
Expand All @@ -45,20 +45,20 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction<T>{
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<T>::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;
Expand All @@ -76,10 +76,10 @@ class RechargeBattery : public rosa_task_plan_bt::RosaAction<T>{

private:
// std::chrono::system_clock::time_point _completion_time;
bool _recharged = false;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr battery_level_sub;
bool recharged_ = false;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr battery_level_sub_;
void battery_level_cb(const std_msgs::msg::Bool &msg){
_recharged = msg.data;
recharged_ = msg.data;
};
};

Expand Down
14 changes: 7 additions & 7 deletions suave_rosa_bt/include/suave_rosa_bt/action_search_pipeline.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,29 +34,29 @@ class SearchPipeline : public rosa_task_plan_bt::RosaAction<T>{
public:
SearchPipeline(
const std::string& name, const BT::NodeConfig & conf)
: rosa_task_plan_bt::RosaAction<T>(name, conf), _pipeline_detected(false)
: rosa_task_plan_bt::RosaAction<T>(name, conf), pipeline_detected_(false)
{
pipeline_detection_sub_ = this->_node->template create_subscription<std_msgs::msg::Bool>(
pipeline_detection_sub_ = this->node_->template create_subscription<std_msgs::msg::Bool>(
"/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<T>::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;
Expand All @@ -73,11 +73,11 @@ class SearchPipeline : public rosa_task_plan_bt::RosaAction<T>{
}

protected:
bool _pipeline_detected;
bool pipeline_detected_;

rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pipeline_detection_sub_;
void pipeline_detected_cb(const std_msgs::msg::Bool &msg){
_pipeline_detected = msg.data;
pipeline_detected_ = msg.data;
};
};

Expand Down
Loading

0 comments on commit f5f7d24

Please sign in to comment.