From 08f25f0514865431868541e27b48297f9fa7346d Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sat, 11 Jul 2026 15:44:40 -0700 Subject: [PATCH 01/10] feat: Behavior tree ChangeStateNode --- .gitmodules | 6 +++++ src/guppy_nav/CMakeLists.txt | 5 ++++ src/guppy_nav/package.xml | 3 +++ src/guppy_state/CMakeLists.txt | 6 ++++- src/guppy_state/package.xml | 2 ++ src/guppy_state/src/change_state_node.cpp | 30 +++++++++++++++++++++++ vendor/behaviortree_cpp | 1 + vendor/behaviortree_cpp.ROS2 | 1 + 8 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 src/guppy_state/src/change_state_node.cpp create mode 160000 vendor/behaviortree_cpp create mode 160000 vendor/behaviortree_cpp.ROS2 diff --git a/.gitmodules b/.gitmodules index 6b66138..5feef13 100644 --- a/.gitmodules +++ b/.gitmodules @@ -11,3 +11,9 @@ [submodule "vendor/gncea"] path = vendor/gncea url = https://github.com/palouserobosub/gncea +[submodule "vendor/behaviortree_cpp"] + path = vendor/behaviortree_cpp + url = https://github.com/BehaviorTree/BehaviorTree.CPP +[submodule "vendor/behaviortree_cpp.ROS2"] + path = vendor/behaviortree_cpp.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2 diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 5e27087..371b365 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -22,6 +22,9 @@ find_package(Eigen3) find_package(control_toolbox REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) + add_library(action_server SHARED src/navigate_action_server.cpp) add_library(pose_setter SHARED src/pose_setter.cpp) @@ -38,6 +41,8 @@ ament_target_dependencies(pose_setter rclcpp rclcpp_action rclcpp_components gup rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) rclcpp_components_register_node(pose_setter PLUGIN "PoseSetterServer" EXECUTABLE the_pose_setter) +target_link_libraries(pose_setter behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2) + install(TARGETS action_server pose_setter diff --git a/src/guppy_nav/package.xml b/src/guppy_nav/package.xml index a33cb6c..d0be9b4 100644 --- a/src/guppy_nav/package.xml +++ b/src/guppy_nav/package.xml @@ -14,6 +14,9 @@ control_toolbox + behaviortree_cpp + behaviortree_ros2 + ament_cmake ament_lint_auto diff --git a/src/guppy_state/CMakeLists.txt b/src/guppy_state/CMakeLists.txt index 4e9feaf..4b8f654 100644 --- a/src/guppy_state/CMakeLists.txt +++ b/src/guppy_state/CMakeLists.txt @@ -18,14 +18,18 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(guppy_msgs REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) -add_executable(state_manager src/state_manager.cpp) +add_executable(state_manager src/state_manager.cpp src/change_state_node.cpp) add_executable(navstart src/navstart.cpp) add_executable(led_pub src/led_pub.cpp) ament_target_dependencies(led_pub rclcpp std_msgs guppy_msgs) ament_target_dependencies(state_manager rclcpp std_srvs std_msgs geometry_msgs guppy_msgs) ament_target_dependencies(navstart rclcpp std_msgs geometry_msgs guppy_msgs) +target_link_libraries(state_manager behaviortree_ros2::behaviortree_ros2) + install(DIRECTORY launch diff --git a/src/guppy_state/package.xml b/src/guppy_state/package.xml index 5a5e212..e2a9b9a 100644 --- a/src/guppy_state/package.xml +++ b/src/guppy_state/package.xml @@ -13,6 +13,8 @@ std_msgs geometry_msgs guppy_msgs + behaviortree_cpp + behaviortree_ros2 ament_lint_auto ament_lint_common diff --git a/src/guppy_state/src/change_state_node.cpp b/src/guppy_state/src/change_state_node.cpp new file mode 100644 index 0000000..b83b249 --- /dev/null +++ b/src/guppy_state/src/change_state_node.cpp @@ -0,0 +1,30 @@ +#include +#include + +class ChangeStateNode : public BT::RosServiceNode +{ +public: + ChangeStateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : RosServiceNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("state") + }); + } + + bool setRequest(Request::SharedPtr& request) override + { + getInput("state", request->new_state); + + return true; + } + + BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override + { + RCLCPP_DEBUG(logger(), "ChangeStateNode success"); + return BT::NodeStatus::SUCCESS; + } +}; \ No newline at end of file diff --git a/vendor/behaviortree_cpp b/vendor/behaviortree_cpp new file mode 160000 index 0000000..4630e06 --- /dev/null +++ b/vendor/behaviortree_cpp @@ -0,0 +1 @@ +Subproject commit 4630e066f842f4f7509d3362b43b651106876cd6 diff --git a/vendor/behaviortree_cpp.ROS2 b/vendor/behaviortree_cpp.ROS2 new file mode 160000 index 0000000..6c6aa07 --- /dev/null +++ b/vendor/behaviortree_cpp.ROS2 @@ -0,0 +1 @@ +Subproject commit 6c6aa078ee7bc52fec98984bed4964556abf5beb From 86fec55eb230554db66f94bbd8898ac6849b4d7a Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sat, 11 Jul 2026 18:33:19 -0700 Subject: [PATCH 02/10] feat: behavior_tree.cpp added --- src/guppy_nav/CMakeLists.txt | 4 +++- src/guppy_nav/src/behavior_tree.cpp | 6 ++++++ src/{guppy_state => guppy_nav}/src/change_state_node.cpp | 0 src/guppy_state/CMakeLists.txt | 6 +----- 4 files changed, 10 insertions(+), 6 deletions(-) create mode 100644 src/guppy_nav/src/behavior_tree.cpp rename src/{guppy_state => guppy_nav}/src/change_state_node.cpp (100%) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 371b365..902e47a 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(behaviortree_ros2 REQUIRED) add_library(action_server SHARED src/navigate_action_server.cpp) add_library(pose_setter SHARED src/pose_setter.cpp) +add_library(behavior_tree SHARED src/behavior_tree.cpp src/change_state_node.cpp) #target_include_directories(action_server PRIVATE # $ @@ -38,10 +39,11 @@ include_directories(include) #add_executable(action_server src/navigate_action_server.cpp) ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) ament_target_dependencies(pose_setter rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) +ament_target_dependencies(behavior_tree guppy_msgs) rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) rclcpp_components_register_node(pose_setter PLUGIN "PoseSetterServer" EXECUTABLE the_pose_setter) -target_link_libraries(pose_setter behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2) +target_link_libraries(behavior_tree behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2) install(TARGETS action_server diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp new file mode 100644 index 0000000..e3fd708 --- /dev/null +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -0,0 +1,6 @@ +#include + +int main() +{ + +} \ No newline at end of file diff --git a/src/guppy_state/src/change_state_node.cpp b/src/guppy_nav/src/change_state_node.cpp similarity index 100% rename from src/guppy_state/src/change_state_node.cpp rename to src/guppy_nav/src/change_state_node.cpp diff --git a/src/guppy_state/CMakeLists.txt b/src/guppy_state/CMakeLists.txt index 4b8f654..4e9feaf 100644 --- a/src/guppy_state/CMakeLists.txt +++ b/src/guppy_state/CMakeLists.txt @@ -18,18 +18,14 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(guppy_msgs REQUIRED) -find_package(behaviortree_cpp REQUIRED) -find_package(behaviortree_ros2 REQUIRED) -add_executable(state_manager src/state_manager.cpp src/change_state_node.cpp) +add_executable(state_manager src/state_manager.cpp) add_executable(navstart src/navstart.cpp) add_executable(led_pub src/led_pub.cpp) ament_target_dependencies(led_pub rclcpp std_msgs guppy_msgs) ament_target_dependencies(state_manager rclcpp std_srvs std_msgs geometry_msgs guppy_msgs) ament_target_dependencies(navstart rclcpp std_msgs geometry_msgs guppy_msgs) -target_link_libraries(state_manager behaviortree_ros2::behaviortree_ros2) - install(DIRECTORY launch From 003d2c973159674a2a8309f265c5fc272c8520d1 Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sat, 11 Jul 2026 19:42:05 -0700 Subject: [PATCH 03/10] feat: add prequal task --- src/guppy_tasks/resource/prequal.xml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 src/guppy_tasks/resource/prequal.xml diff --git a/src/guppy_tasks/resource/prequal.xml b/src/guppy_tasks/resource/prequal.xml new file mode 100644 index 0000000..7a4e2d2 --- /dev/null +++ b/src/guppy_tasks/resource/prequal.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file From 8962e78e689b3e5b741a9e83da3d3beafd5778ed Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sat, 11 Jul 2026 19:51:55 -0700 Subject: [PATCH 04/10] fix: state taking float --- src/guppy_nav/src/change_state_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/guppy_nav/src/change_state_node.cpp b/src/guppy_nav/src/change_state_node.cpp index b83b249..e7f1422 100644 --- a/src/guppy_nav/src/change_state_node.cpp +++ b/src/guppy_nav/src/change_state_node.cpp @@ -11,13 +11,15 @@ class ChangeStateNode : public BT::RosServiceNode static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("state") + BT::InputPort("state") }); } bool setRequest(Request::SharedPtr& request) override { - getInput("state", request->new_state); + getInput("state", request->new_state.state); + + RCLCPP_DEBUG(logger(), "Request to change State"); return true; } From 4d86cf8a3c14aa5d74b59860f7a0c6ff17034abb Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sat, 11 Jul 2026 19:57:57 -0700 Subject: [PATCH 05/10] feat: navigation action node for behaviourtree.cpp --- src/guppy_nav/src/pose_setter_behavior.cpp | 43 ++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 src/guppy_nav/src/pose_setter_behavior.cpp diff --git a/src/guppy_nav/src/pose_setter_behavior.cpp b/src/guppy_nav/src/pose_setter_behavior.cpp new file mode 100644 index 0000000..69a4590 --- /dev/null +++ b/src/guppy_nav/src/pose_setter_behavior.cpp @@ -0,0 +1,43 @@ +#include "guppy_msgs/action/navigate.hpp" + +#include + +class NavigateAction: public BT::RosActionNode { +public: + NavigateAction(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode(name, conf, params) {} + + static BT::PortList providedPorts() { + return providedBasicPorts({ + BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("z"), + BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), + BT::InputPort("local"), BT::InputPort("timeout") + }); + } + + bool setGoal(BT::RosActionNode::Goal& goal) override { + getInput("x", goal.pose.position.x); + getInput("y", goal.pose.position.y); + getInput("z", goal.pose.position.z); + getInput("qw", goal.pose.orientation.w); + getInput("qx", goal.pose.orientation.x); + getInput("qy", goal.pose.orientation.y); + getInput("qz", goal.pose.orientation.z); + getInput("local", goal.local); + getInput("timeout", goal.timeout); + return true; + } + + BT::NodeStatus onResultReceived(const BT::WrappedResult& wrapped) override { + // should do? + return BT::NodeStatus::SUCCESS; + } + + virtual NodeStatus onFailure(BT::ActionNodeErrorCode error) override { + RCLCPP_ERROR(logger(), "error: %d", error); + return NodeStatus::FAILURE; + } + + BT::NodeStatus onFeedback(const std::shared_ptr feedback) { + return BT::NodeStatus::RUNNING; + } +}; From 968d0be68969e44653a99d5445db5d803d97e9e0 Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sun, 12 Jul 2026 01:38:28 -0700 Subject: [PATCH 06/10] feat: prequal behavior tree starts on state change to NAV --- src/guppy_nav/CMakeLists.txt | 4 +- .../guppy_nav/change_state_behavior.hpp} | 8 ++- .../guppy_nav/pose_setter_behavior.hpp} | 18 ++--- src/guppy_nav/src/behavior_tree.cpp | 67 +++++++++++++++++-- src/guppy_tasks/resource/prequal.xml | 11 ++- 5 files changed, 84 insertions(+), 24 deletions(-) rename src/guppy_nav/{src/change_state_node.cpp => include/guppy_nav/change_state_behavior.hpp} (77%) rename src/guppy_nav/{src/pose_setter_behavior.cpp => include/guppy_nav/pose_setter_behavior.hpp} (61%) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 902e47a..bfa8d72 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(behaviortree_ros2 REQUIRED) add_library(action_server SHARED src/navigate_action_server.cpp) add_library(pose_setter SHARED src/pose_setter.cpp) -add_library(behavior_tree SHARED src/behavior_tree.cpp src/change_state_node.cpp) +add_library(behavior_tree SHARED src/behavior_tree.cpp) #target_include_directories(action_server PRIVATE # $ @@ -57,7 +57,7 @@ install(TARGETS install(DIRECTORY launch - + DESTINATION share/${PROJECT_NAME}/ ) diff --git a/src/guppy_nav/src/change_state_node.cpp b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp similarity index 77% rename from src/guppy_nav/src/change_state_node.cpp rename to src/guppy_nav/include/guppy_nav/change_state_behavior.hpp index e7f1422..923d793 100644 --- a/src/guppy_nav/src/change_state_node.cpp +++ b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp @@ -1,10 +1,12 @@ +#pragma once + #include #include -class ChangeStateNode : public BT::RosServiceNode +class ChangeStateBehavior : public BT::RosServiceNode { public: - ChangeStateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + ChangeStateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : RosServiceNode(name, conf, params) {} @@ -29,4 +31,4 @@ class ChangeStateNode : public BT::RosServiceNode RCLCPP_DEBUG(logger(), "ChangeStateNode success"); return BT::NodeStatus::SUCCESS; } -}; \ No newline at end of file +}; diff --git a/src/guppy_nav/src/pose_setter_behavior.cpp b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp similarity index 61% rename from src/guppy_nav/src/pose_setter_behavior.cpp rename to src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp index 69a4590..ee543a7 100644 --- a/src/guppy_nav/src/pose_setter_behavior.cpp +++ b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp @@ -1,12 +1,14 @@ +#pragma once + #include "guppy_msgs/action/navigate.hpp" #include -class NavigateAction: public BT::RosActionNode { +class NavigateBehavior: public BT::RosActionNode { public: - NavigateAction(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode(name, conf, params) {} + NavigateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode(name, conf, params) {} - static BT::PortList providedPorts() { + static BT::PortsList providedPorts() { return providedBasicPorts({ BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("z"), BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), @@ -14,7 +16,7 @@ class NavigateAction: public BT::RosActionNode { }); } - bool setGoal(BT::RosActionNode::Goal& goal) override { + bool setGoal(BT::RosActionNode::Goal& goal) override { getInput("x", goal.pose.position.x); getInput("y", goal.pose.position.y); getInput("z", goal.pose.position.z); @@ -27,17 +29,17 @@ class NavigateAction: public BT::RosActionNode { return true; } - BT::NodeStatus onResultReceived(const BT::WrappedResult& wrapped) override { + BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override { // should do? return BT::NodeStatus::SUCCESS; } - virtual NodeStatus onFailure(BT::ActionNodeErrorCode error) override { + virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { RCLCPP_ERROR(logger(), "error: %d", error); - return NodeStatus::FAILURE; + return BT::NodeStatus::FAILURE; } - BT::NodeStatus onFeedback(const std::shared_ptr feedback) { + BT::NodeStatus onFeedback(const std::shared_ptr feedback) { return BT::NodeStatus::RUNNING; } }; diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp index e3fd708..467a67a 100644 --- a/src/guppy_nav/src/behavior_tree.cpp +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -1,6 +1,63 @@ -#include +#include +#include -int main() -{ - -} \ No newline at end of file +#include +#include + +#include +#include + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "guppy_msgs/msg/state.hpp" + +class NavigationBehaviorTree : public rclcpp::Node { +public: + NavigationBehaviorTree() : Node("navigation_behavior_tree") { + BT::BehaviorTreeFactory factory; + + BT::RosNodeParams stateParameters(std::make_shared("chage_state_behavior_client"), "change_state"); + BT::RosNodeParams navigateParameters(std::make_shared("navigate_behavior_client"), "/navigate"); + factory.registerNodeType("ChangeState", stateParameters); + factory.registerNodeType("Navigate", navigateParameters); + + _tree = std::make_unique(factory.createTreeFromFile("./src/guppy_tasks/resource/prequal.xml")); + + auto tick = [this]() { + if (!_running) return; + _tree->tickOnce(); + }; + + auto onState = [this](guppy_msgs::msg::State::UniquePtr msg) { + auto nav = msg->state == guppy_msgs::msg::State::NAV; + if (nav && !_running) _running = true; + else if (!nav && _running) { + _running = false; + _tree->haltTree(); + } + }; + + _timer = this->create_wall_timer(std::chrono::milliseconds(20), tick); + + auto state_quality = rclcpp::QoS(rclcpp::KeepLast(1)).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE).durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + _subscription = this->create_subscription("state", state_quality, onState); + } +private: + std::unique_ptr _tree = nullptr; + + rclcpp::Subscription::SharedPtr _subscription; + rclcpp::TimerBase::SharedPtr _timer; + + bool _running = false; +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/guppy_tasks/resource/prequal.xml b/src/guppy_tasks/resource/prequal.xml index 7a4e2d2..839905a 100644 --- a/src/guppy_tasks/resource/prequal.xml +++ b/src/guppy_tasks/resource/prequal.xml @@ -1,12 +1,11 @@ - - - - - + + + + - \ No newline at end of file + From 1eebc483901c88e1aec5326143d7d4d8a1dee437 Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sun, 12 Jul 2026 08:06:03 -0700 Subject: [PATCH 07/10] fix: cleaned up CMakeLists and added behavior tree to launch file --- src/guppy_nav/CMakeLists.txt | 54 +++++++++++++------ src/guppy_nav/launch/core.xml | 7 +-- src/guppy_nav/src/behavior_tree.cpp | 4 +- ...pose_setter.cpp => pose_setter_server.cpp} | 0 4 files changed, 45 insertions(+), 20 deletions(-) rename src/guppy_nav/src/{pose_setter.cpp => pose_setter_server.cpp} (100%) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index bfa8d72..66def42 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) @@ -25,29 +26,50 @@ find_package(control_toolbox REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(behaviortree_ros2 REQUIRED) -add_library(action_server SHARED src/navigate_action_server.cpp) -add_library(pose_setter SHARED src/pose_setter.cpp) -add_library(behavior_tree SHARED src/behavior_tree.cpp) +#add_library(navigate_action SHARED src/navigate_action_server.cpp) +add_library(pose_setter SHARED src/pose_setter_server.cpp) +add_executable(behavior_tree src/behavior_tree.cpp) -#target_include_directories(action_server PRIVATE -# $ -# $ +#target_include_directories(navigate_action_server PUBLIC +# $ +# $ #) +target_include_directories(pose_setter PUBLIC + $ + $ +) +target_include_directories(behavior_tree PUBLIC + $ + $ +) -include_directories(include) +#ament_target_dependencies(navigate_action +# rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox +#) +ament_target_dependencies(pose_setter + rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox +) +ament_target_dependencies(behavior_tree + guppy_msgs +) -#add_executable(action_server src/navigate_action_server.cpp) -ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) -ament_target_dependencies(pose_setter rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) -ament_target_dependencies(behavior_tree guppy_msgs) -rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) -rclcpp_components_register_node(pose_setter PLUGIN "PoseSetterServer" EXECUTABLE the_pose_setter) +target_link_libraries(behavior_tree + behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2 +) -target_link_libraries(behavior_tree behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2) +#rclcpp_components_register_node(navigate_action +# PLUGIN "guppy_nav::NavigateActionServer" +# EXECUTABLE navigate_action_server +#) +rclcpp_components_register_node(pose_setter + PLUGIN "guppy_nav::PoseSetterServer" + EXECUTABLE pose_setter_server +) install(TARGETS - action_server - pose_setter + #navigate_action navigate_action_server + pose_setter pose_setter_server + behavior_tree LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/src/guppy_nav/launch/core.xml b/src/guppy_nav/launch/core.xml index 551c06b..b7cb25a 100644 --- a/src/guppy_nav/launch/core.xml +++ b/src/guppy_nav/launch/core.xml @@ -1,5 +1,6 @@ - - - \ No newline at end of file + + + + diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp index 467a67a..bdde0f4 100644 --- a/src/guppy_nav/src/behavior_tree.cpp +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -13,6 +13,8 @@ #include "behaviortree_cpp/bt_factory.h" #include "guppy_msgs/msg/state.hpp" +#define TICK_MS 20 + class NavigationBehaviorTree : public rclcpp::Node { public: NavigationBehaviorTree() : Node("navigation_behavior_tree") { @@ -39,7 +41,7 @@ class NavigationBehaviorTree : public rclcpp::Node { } }; - _timer = this->create_wall_timer(std::chrono::milliseconds(20), tick); + _timer = this->create_wall_timer(std::chrono::milliseconds(TICK_MS), tick); auto state_quality = rclcpp::QoS(rclcpp::KeepLast(1)).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE).durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); _subscription = this->create_subscription("state", state_quality, onState); diff --git a/src/guppy_nav/src/pose_setter.cpp b/src/guppy_nav/src/pose_setter_server.cpp similarity index 100% rename from src/guppy_nav/src/pose_setter.cpp rename to src/guppy_nav/src/pose_setter_server.cpp From e975341fe5627c5ba56f57b3bc4c8c19e559eee8 Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Sun, 12 Jul 2026 21:21:17 -0700 Subject: [PATCH 08/10] feat: fallback behavior in tree --- src/guppy_nav/CMakeLists.txt | 15 +++++++++---- .../guppy_nav/pose_setter_behavior.hpp | 14 +++++++----- src/guppy_nav/src/behavior_tree.cpp | 12 +++++++--- src/guppy_nav/src/pose_setter_server.cpp | 8 +++---- src/guppy_state/src/state_manager.cpp | 22 ++++++++----------- src/guppy_tasks/resource/prequal.xml | 19 +++++++++------- 6 files changed, 53 insertions(+), 37 deletions(-) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 66def42..bc23d78 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -43,6 +43,13 @@ target_include_directories(behavior_tree PUBLIC $ ) +#target_compile_definitions(navigate_action_server +# PRIVATE "GUPPY_NAV_BUILDING_DLL" +#) +target_compile_definitions(pose_setter + PRIVATE "GUPPY_NAV_BUILDING_DLL" +) + #ament_target_dependencies(navigate_action # rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox #) @@ -50,7 +57,7 @@ ament_target_dependencies(pose_setter rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox ) ament_target_dependencies(behavior_tree - guppy_msgs + rclcpp guppy_msgs behaviortree_cpp behaviortree_ros2 ) target_link_libraries(behavior_tree @@ -58,11 +65,11 @@ target_link_libraries(behavior_tree ) #rclcpp_components_register_node(navigate_action -# PLUGIN "guppy_nav::NavigateActionServer" +# PLUGIN "NavigateActionServer" # EXECUTABLE navigate_action_server #) rclcpp_components_register_node(pose_setter - PLUGIN "guppy_nav::PoseSetterServer" + PLUGIN "PoseSetterServer" EXECUTABLE pose_setter_server ) @@ -74,7 +81,7 @@ install(TARGETS LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - #DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY diff --git a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp index ee543a7..e110bc7 100644 --- a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp +++ b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp @@ -10,9 +10,10 @@ class NavigateBehavior: public BT::RosActionNode { static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("z"), - BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), BT::InputPort("qw"), - BT::InputPort("local"), BT::InputPort("timeout") + BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("z"), + BT::InputPort("qw"), BT::InputPort("qx"), BT::InputPort("qy"), BT::InputPort("qz"), + BT::InputPort("local"), BT::InputPort("timeout"), + BT::InputPort("continueOnTimeout") }); } @@ -35,8 +36,11 @@ class NavigateBehavior: public BT::RosActionNode { } virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { - RCLCPP_ERROR(logger(), "error: %d", error); - return BT::NodeStatus::FAILURE; + RCLCPP_ERROR(logger(), "pose setter treenode error... %s", BT::toStr(error)); + bool continueOnTimeout = false; + getInput("continueOnTimeout", continueOnTimeout); + if (continueOnTimeout) return BT::NodeStatus::SUCCESS; + else return BT::NodeStatus::FAILURE; } BT::NodeStatus onFeedback(const std::shared_ptr feedback) { diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp index bdde0f4..1c82319 100644 --- a/src/guppy_nav/src/behavior_tree.cpp +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -20,8 +20,11 @@ class NavigationBehaviorTree : public rclcpp::Node { NavigationBehaviorTree() : Node("navigation_behavior_tree") { BT::BehaviorTreeFactory factory; - BT::RosNodeParams stateParameters(std::make_shared("chage_state_behavior_client"), "change_state"); - BT::RosNodeParams navigateParameters(std::make_shared("navigate_behavior_client"), "/navigate"); + _change_state_client = std::make_shared("chage_state_behavior_client"); + _navigation_client = std::make_shared("navigate_behavior_client"); + + BT::RosNodeParams stateParameters(_change_state_client, "change_state"); + BT::RosNodeParams navigateParameters(_navigation_client, "/navigate"); factory.registerNodeType("ChangeState", stateParameters); factory.registerNodeType("Navigate", navigateParameters); @@ -47,7 +50,10 @@ class NavigationBehaviorTree : public rclcpp::Node { _subscription = this->create_subscription("state", state_quality, onState); } private: - std::unique_ptr _tree = nullptr; + std::unique_ptr _tree; + + std::shared_ptr _change_state_client; + std::shared_ptr _navigation_client; rclcpp::Subscription::SharedPtr _subscription; rclcpp::TimerBase::SharedPtr _timer; diff --git a/src/guppy_nav/src/pose_setter_server.cpp b/src/guppy_nav/src/pose_setter_server.cpp index 9e42391..f84439d 100644 --- a/src/guppy_nav/src/pose_setter_server.cpp +++ b/src/guppy_nav/src/pose_setter_server.cpp @@ -50,7 +50,7 @@ class PoseSetterServer : public rclcpp::Node { execute(goalHandle); }}.detach(); }; - + _actionServer = rclcpp_action::create_server( this, "/navigate", @@ -68,7 +68,7 @@ class PoseSetterServer : public rclcpp::Node { _current_pos = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); _current_quat = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); } - + void stateCallback(guppy_msgs::msg::State::SharedPtr msg) { _state = msg->state; } @@ -193,7 +193,7 @@ class PoseSetterServer : public rclcpp::Node { if (rclcpp::ok()) { result->pose = get_current_pose(); - result->target_reached = true;\ + result->target_reached = true; result->error = pose_from_vec_quat(error, qerror); goalHandle->succeed(result); return; @@ -216,4 +216,4 @@ int main(int argc, char* argv[]) { rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/guppy_state/src/state_manager.cpp b/src/guppy_state/src/state_manager.cpp index 769d385..ae47e30 100644 --- a/src/guppy_state/src/state_manager.cpp +++ b/src/guppy_state/src/state_manager.cpp @@ -46,7 +46,7 @@ class StateManager : public rclcpp::Node { static const geometry_msgs::msg::Twist zero_twist; } - + private: void estopcallback(guppy_msgs::msg::CanFrame msg) { int is_estopped = 0; @@ -80,8 +80,8 @@ class StateManager : public rclcpp::Node { const std::shared_ptr request, std::shared_ptr response ) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "State transition request..."); - + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "State transition request..."); + if (!is_valid_state(request->new_state.state)) { RCLCPP_ERROR(this->get_logger(), "Invalid state passed in transition service!"); response->success = false; @@ -94,20 +94,16 @@ class StateManager : public rclcpp::Node { return; } - if (this->current_state_ == guppy_msgs::msg::State::NAV) { - system("killall prequal"); - } - if (request->new_state.state == guppy_msgs::msg::State::HOLDING) { auto request = std::make_shared(); resetholdpose->async_send_request(request); } // TODO switch logic should be handled here NOT in StateManager#publishState() - - response->success = this->publish_state(request->new_state.state); + + response->success = this->publish_state(request->new_state.state); } - + bool publish_state(uint8_t state) { auto message = guppy_msgs::msg::State(); message.state = state; @@ -162,7 +158,7 @@ class StateManager : public rclcpp::Node { void handle_fault() { // TODO } - + uint8_t current_state_; rclcpp::Publisher::SharedPtr state_publisher_; rclcpp::Subscription::SharedPtr estopsubscription_; @@ -182,7 +178,7 @@ class StateManager : public rclcpp::Node { rclcpp::Publisher::SharedPtr cmd_vel_publisher_; bool was_estopped = false; - + const geometry_msgs::msg::Twist zero_twist = []() { geometry_msgs::msg::Vector3 zero_vector; zero_vector.x = 0.0; @@ -200,7 +196,7 @@ int main(int argc, char* argv[]) { rclcpp::init(argc, argv); auto publisher_node = std::make_shared(); - + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "State transition server ready."); rclcpp::spin(publisher_node); diff --git a/src/guppy_tasks/resource/prequal.xml b/src/guppy_tasks/resource/prequal.xml index 839905a..d789e82 100644 --- a/src/guppy_tasks/resource/prequal.xml +++ b/src/guppy_tasks/resource/prequal.xml @@ -1,11 +1,14 @@ - - - - - - - - + + + + + + + + + + + From b8ea44325ae8b6cd4340f9de045e4fe973748e3f Mon Sep 17 00:00:00 2001 From: ryden-handsaker Date: Mon, 13 Jul 2026 13:47:44 -0700 Subject: [PATCH 09/10] feat: added t-shape and nav tree nodes --- .../include/guppy_nav/acquire_detection.hpp | 40 ++++++++++ .../guppy_nav/face_detection_behavior.hpp | 75 +++++++++++++++++++ .../guppy_nav/pose_setter_behavior.hpp | 10 ++- src/guppy_nav/src/behavior_tree.cpp | 13 +++- src/guppy_nav/src/navigate_action_server.cpp | 10 +-- src/guppy_state/src/state_manager.cpp | 8 +- src/guppy_tasks/resource/guppy_tasks | 5 ++ src/guppy_tasks/resource/search.xml | 10 +++ src/guppy_tasks/resource/t-shape.xml | 17 +++++ 9 files changed, 178 insertions(+), 10 deletions(-) create mode 100644 src/guppy_nav/include/guppy_nav/acquire_detection.hpp create mode 100644 src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp create mode 100644 src/guppy_tasks/resource/search.xml create mode 100644 src/guppy_tasks/resource/t-shape.xml diff --git a/src/guppy_nav/include/guppy_nav/acquire_detection.hpp b/src/guppy_nav/include/guppy_nav/acquire_detection.hpp new file mode 100644 index 0000000..40c9044 --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/acquire_detection.hpp @@ -0,0 +1,40 @@ +#include + +#include +#include +#include + +#include +#include "guppy_msgs/msg/corner_detection.hpp" +#include "guppy_msgs/msg/corner_detection_list.hpp" + +class AcquireDetection : public BT::RosTopicSubNode { +public: + AcquireDetection(const std::string& name, const BT::NodeConfig& config, const BT::RosNodeParams& params) + : BT::RosTopicSubNode(name, config, params) { } + + static BT::PortsList providedPorts() { + return providedBasicPorts({ + BT::InputPort("name"), + BT::OutputPort("detection") + }); + } + + BT::NodeStatus onTick(const std::shared_ptr& msg) override { + if (!msg) return BT::NodeStatus::FAILURE; // no detections in list + + std::string target; + getInput("target", target); + + auto it = std::find_if(msg->detections.begin(), msg->detections.end(), [target](const guppy_msgs::msg::CornerDetection detection) { return matchesTarget(detection, target); }); + if (it == msg->detections.end()) return BT::NodeStatus::FAILURE; // no detection matching target + + setOutput("detection", *it); + + return BT::NodeStatus::SUCCESS; + } +private: + static bool matchesTarget(const guppy_msgs::msg::CornerDetection detection, const std::string& target) { + return detection.name == target; + } +}; diff --git a/src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp b/src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp new file mode 100644 index 0000000..5ba2024 --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include "guppy_msgs/action/navigate.hpp" +#include "guppy_msgs/msg/corner_detection.hpp" + +#include + +#include +#include + +#define CAMERA_RES_X 1920 +#define CAMERA_RES_Y 1080 + +#define CAMERA_MAX_ANGLE_YAW 1.57079632679 +#define CAMERA_MAX_ANGLE_PITCH 1.57079632679 + +class FaceDetectionBehavior: public BT::RosActionNode { +public: + FaceDetectionBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : BT::RosActionNode(name, conf, params) {} + + static BT::PortsList providedPorts() { + return providedBasicPorts({ + BT::InputPort("detection"), + BT::InputPort("timeout"), BT::InputPort("continueOnTimeout") + }); + } + + bool setGoal(BT::RosActionNode::Goal& goal) override { + guppy_msgs::msg::CornerDetection detection; + getInput("detection", detection); + + auto x = 0.0, y = 0.0; + for (auto corner : detection.corners) x += corner.x, y += corner.y; + + auto size = detection.corners.size(); + x /= size, y /= size; + + auto roll = 0.0, pitch = CAMERA_MAX_ANGLE_PITCH * (y / CAMERA_RES_Y), + yaw = CAMERA_MAX_ANGLE_YAW * (x / CAMERA_RES_X); + Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + goal.pose.position.x = goal.pose.position.y = goal.pose.position.z = 0.0; // doesn't move position + goal.pose.orientation.w = q.w(), goal.pose.orientation.x = q.x(), + goal.pose.orientation.y = q.y(),goal.pose.orientation.z = q.z(); + + goal.local = true; // local to cameras so has to be local + + getInput("timeout", goal.timeout); + return true; + } + + BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override { + // should do? + return BT::NodeStatus::SUCCESS; + } + + virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { + bool continueOnTimeout = false; + getInput("continueOnTimeout", continueOnTimeout); + if (continueOnTimeout) { + RCLCPP_INFO(logger(), "pose setter action aborted, continuing..."); + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(logger(), "pose setter node error... %s", BT::toStr(error)); + return BT::NodeStatus::FAILURE; + } + } + + BT::NodeStatus onFeedback(const std::shared_ptr feedback) { + return BT::NodeStatus::RUNNING; + } +}; diff --git a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp index e110bc7..163baeb 100644 --- a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp +++ b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp @@ -36,11 +36,15 @@ class NavigateBehavior: public BT::RosActionNode { } virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { - RCLCPP_ERROR(logger(), "pose setter treenode error... %s", BT::toStr(error)); bool continueOnTimeout = false; getInput("continueOnTimeout", continueOnTimeout); - if (continueOnTimeout) return BT::NodeStatus::SUCCESS; - else return BT::NodeStatus::FAILURE; + if (continueOnTimeout) { + RCLCPP_INFO(logger(), "navigate action aborted, continuing..."); + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(logger(), "pose setter treenode error... %s", BT::toStr(error)); + return BT::NodeStatus::FAILURE; + } } BT::NodeStatus onFeedback(const std::shared_ptr feedback) { diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp index 1c82319..1388401 100644 --- a/src/guppy_nav/src/behavior_tree.cpp +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -9,11 +10,15 @@ #include #include +#include +#include #include "behaviortree_cpp/bt_factory.h" #include "guppy_msgs/msg/state.hpp" +#include "guppy_nav/face_detection_behavior.hpp" #define TICK_MS 20 +#define TREE_NAME "t-shape" class NavigationBehaviorTree : public rclcpp::Node { public: @@ -22,13 +27,18 @@ class NavigationBehaviorTree : public rclcpp::Node { _change_state_client = std::make_shared("chage_state_behavior_client"); _navigation_client = std::make_shared("navigate_behavior_client"); + _detection_subscriber = std::make_shared("detection_subscriber"); BT::RosNodeParams stateParameters(_change_state_client, "change_state"); BT::RosNodeParams navigateParameters(_navigation_client, "/navigate"); + BT::RosNodeParams detectionParameters(_detection_subscriber, "/cam/test/detections"); + factory.registerNodeType("ChangeState", stateParameters); factory.registerNodeType("Navigate", navigateParameters); + factory.registerNodeType("FaceDetection", navigateParameters); + factory.registerNodeType("AcquireDetection", detectionParameters); - _tree = std::make_unique(factory.createTreeFromFile("./src/guppy_tasks/resource/prequal.xml")); + _tree = std::make_unique(factory.createTreeFromFile("./src/guppy_tasks/resource/" + std::string(TREE_NAME) + ".xml")); auto tick = [this]() { if (!_running) return; @@ -54,6 +64,7 @@ class NavigationBehaviorTree : public rclcpp::Node { std::shared_ptr _change_state_client; std::shared_ptr _navigation_client; + std::shared_ptr _detection_subscriber; rclcpp::Subscription::SharedPtr _subscription; rclcpp::TimerBase::SharedPtr _timer; diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 3570a96..eae723a 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -55,7 +55,7 @@ class NavigateActionServer : public rclcpp::Node { execute(goalHandle); }}.detach(); }; - + _actionServer = rclcpp_action::create_server( this, "/navigate", @@ -107,7 +107,7 @@ class NavigateActionServer : public rclcpp::Node { struct Trajectory3 { Trajectory x, y, z; - + explicit Trajectory3(const Eigen::Vector3d& startVelocity, const Eigen::Vector3d& endVelocity, double attack, double decay, double totalTime, const Eigen::Vector3d& targetPosition) : x(Trajectory(startVelocity.x(), endVelocity.x(), attack, decay, totalTime, targetPosition.x())), y(Trajectory(startVelocity.y(), endVelocity.y(), attack, decay, totalTime, targetPosition.y())), @@ -200,7 +200,7 @@ class NavigateActionServer : public rclcpp::Node { Eigen::Quaterniond finalOrientation; // world if (goal->local) { finalPosition = initialPosition + initialOrientation.inverse() * goalPosition; // get final position in world based on guppy's position/orientation - finalOrientation = initialOrientation * goalOrientation; + finalOrientation = initialOrientation * goalOrientation; } else { finalPosition = goalPosition; finalOrientation = goalOrientation; @@ -224,7 +224,7 @@ class NavigateActionServer : public rclcpp::Node { auto clock = this->get_clock(); rclcpp::Time start = clock->now(); rclcpp::Time last = start; - + auto feedback = std::make_shared(); auto result = std::make_shared(); @@ -290,4 +290,4 @@ int main(int argc, char* argv[]) { rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/guppy_state/src/state_manager.cpp b/src/guppy_state/src/state_manager.cpp index ae47e30..6bbab35 100644 --- a/src/guppy_state/src/state_manager.cpp +++ b/src/guppy_state/src/state_manager.cpp @@ -80,7 +80,13 @@ class StateManager : public rclcpp::Node { const std::shared_ptr request, std::shared_ptr response ) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "State transition request..."); + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "State transition request..."); + + if (request->new_state.state == this->current_state_) { + RCLCPP_ERROR(this->get_logger(), "Already in state!"); + response->success = false; + return; + } if (!is_valid_state(request->new_state.state)) { RCLCPP_ERROR(this->get_logger(), "Invalid state passed in transition service!"); diff --git a/src/guppy_tasks/resource/guppy_tasks b/src/guppy_tasks/resource/guppy_tasks index e69de29..5fee7d4 100644 --- a/src/guppy_tasks/resource/guppy_tasks +++ b/src/guppy_tasks/resource/guppy_tasks @@ -0,0 +1,5 @@ +async sequence + - search pattern, constantly moving + - wait 5 seconds, check if ayny images + - if images halt search and move toward + - if no images, repeat. diff --git a/src/guppy_tasks/resource/search.xml b/src/guppy_tasks/resource/search.xml new file mode 100644 index 0000000..617abc9 --- /dev/null +++ b/src/guppy_tasks/resource/search.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/guppy_tasks/resource/t-shape.xml b/src/guppy_tasks/resource/t-shape.xml new file mode 100644 index 0000000..81d91f3 --- /dev/null +++ b/src/guppy_tasks/resource/t-shape.xml @@ -0,0 +1,17 @@ + + +