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..bc23d78 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) @@ -22,35 +23,70 @@ find_package(Eigen3) find_package(control_toolbox REQUIRED) -add_library(action_server SHARED src/navigate_action_server.cpp) -add_library(pose_setter SHARED src/pose_setter.cpp) +find_package(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) -#target_include_directories(action_server PRIVATE -# $ -# $ +#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(navigate_action_server PUBLIC +# $ +# $ +#) +target_include_directories(pose_setter PUBLIC + $ + $ +) +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 #) +ament_target_dependencies(pose_setter + rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox +) +ament_target_dependencies(behavior_tree + rclcpp guppy_msgs behaviortree_cpp behaviortree_ros2 +) -include_directories(include) +target_link_libraries(behavior_tree + behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2 +) -#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) -rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) -rclcpp_components_register_node(pose_setter PLUGIN "PoseSetterServer" EXECUTABLE the_pose_setter) +#rclcpp_components_register_node(navigate_action +# PLUGIN "NavigateActionServer" +# EXECUTABLE navigate_action_server +#) +rclcpp_components_register_node(pose_setter + PLUGIN "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 RUNTIME DESTINATION bin - #DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch - + DESTINATION share/${PROJECT_NAME}/ ) 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..c8f1ca0 --- /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("target"), + 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/change_state_behavior.hpp b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp new file mode 100644 index 0000000..923d793 --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +class ChangeStateBehavior : public BT::RosServiceNode +{ +public: + ChangeStateBehavior(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.state); + + RCLCPP_DEBUG(logger(), "Request to change State"); + + return true; + } + + BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override + { + RCLCPP_DEBUG(logger(), "ChangeStateNode success"); + return BT::NodeStatus::SUCCESS; + } +}; 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..c82efcf --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp @@ -0,0 +1,77 @@ +#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 + +// https://www.desmos.com/calculator/ivz6gpks8n + +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 new file mode 100644 index 0000000..163baeb --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "guppy_msgs/action/navigate.hpp" + +#include + +class NavigateBehavior: public BT::RosActionNode { +public: + NavigateBehavior(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("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") + }); + } + + 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 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(), "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) { + return BT::NodeStatus::RUNNING; + } +}; 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/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_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp new file mode 100644 index 0000000..1388401 --- /dev/null +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -0,0 +1,82 @@ +#include +#include +#include + +#include +#include + +#include +#include + +#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: + NavigationBehaviorTree() : Node("navigation_behavior_tree") { + BT::BehaviorTreeFactory factory; + + _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/" + std::string(TREE_NAME) + ".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(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); + } +private: + std::unique_ptr _tree; + + std::shared_ptr _change_state_client; + std::shared_ptr _navigation_client; + std::shared_ptr _detection_subscriber; + + 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_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_nav/src/pose_setter.cpp b/src/guppy_nav/src/pose_setter_server.cpp similarity index 99% rename from src/guppy_nav/src/pose_setter.cpp rename to src/guppy_nav/src/pose_setter_server.cpp index 9e42391..f84439d 100644 --- a/src/guppy_nav/src/pose_setter.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/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/state_manager.cpp b/src/guppy_state/src/state_manager.cpp index 769d385..6bbab35 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,14 @@ 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!"); response->success = false; @@ -94,20 +100,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 +164,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 +184,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 +202,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/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/prequal.xml b/src/guppy_tasks/resource/prequal.xml new file mode 100644 index 0000000..d789e82 --- /dev/null +++ b/src/guppy_tasks/resource/prequal.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + 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..35798fc --- /dev/null +++ b/src/guppy_tasks/resource/t-shape.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + 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