Greg/lanefollowslalom actionstuff

ros2_env.bash

@@ -0,0 +1,3 @@
+source /opt/ros/jazzy/setup.bash
+export QT_QPA_PLATFORM=xcb
+export GDK_BACKEND=x11

src/guppy_msgs/CMakeLists.txt

@@ -29,6 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TransformList.msg"
 
   "action/Navigate.action"
+  "action/LaneFollow.action"
 
   DEPENDENCIES builtin_interfaces
   DEPENDENCIES geometry_msgs

src/guppy_msgs/action/LaneFollow.action

@@ -0,0 +1,8 @@
+float64 lost_timeout
+---
+bool success
+string message
+---
+bool lane_found
+float64 x_center
+float64 depth

src/guppy_nav/include/guppy_nav/lane_follow_behavior.hpp

@@ -0,0 +1,39 @@
+
+#pragma once
+
+#include <behaviortree_ros2/bt_action_node.hpp>
+#include "guppy_msgs/action/lane_follow.hpp"
+
+class LaneFollowBehavior : public BT::RosActionNode<guppy_msgs::action::LaneFollow>{
+    public:
+        LaneFollowBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode<guppy_msgs::action::LaneFollow>(name,conf,params){}
+
+    static BT::PortsList providedPorts(){
+        return providedBasicPorts({
+            BT::InputPort<double>("lost_timeout",2.0,"seconds without a lane sighted befoe the action fails")
+
+        });
+    }
+
+    bool setGoal(RosActionNode::Goal& goal)override{
+        getInput("lost_timeout",goal.lost_timeout);
+        return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult& wr) override{
+        if (wr.result->success){
+            RCLCPP_INFO(logger(),"LaneFolow succeeded : %s",wr.result->message.c_str());
+            return BT::NodeStatus::SUCCESS;
+        }
+        return BT::NodeStatus::FAILURE;
+    }
+    BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) {
+        (void)feedback;
+        return BT::NodeStatus::RUNNING;
+    }
+
+    BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override{
+        RCLCPP_ERROR(logger(), "LaneFollow action error: %d",static_cast<int>(error));
+        return BT::NodeStatus::FAILURE;
+    }
+};

src/guppy_nav/src/behavior_tree.cpp

@@ -17,8 +17,11 @@
 #include "guppy_msgs/msg/state.hpp"
 #include "guppy_nav/face_detection_behavior.hpp"
 
+#include <guppy_nav/lane_follow_behavior.hpp>
+
 #define TICK_MS 20
-#define TREE_NAME "t-shape"
+// #define TREE_NAME "t-shape"
+#define TREE_NAME "LaneFollow" //I CHANGE THIS BACK TO WHATEVVER I GUESS
 
 class NavigationBehaviorTree : public rclcpp::Node {
 public:
@@ -28,15 +31,18 @@ class NavigationBehaviorTree : public rclcpp::Node {
         _change_state_client = std::make_shared<rclcpp::Node>("chage_state_behavior_client");
         _navigation_client = std::make_shared<rclcpp::Node>("navigate_behavior_client");
         _detection_subscriber = std::make_shared<rclcpp::Node>("detection_subscriber");
+        _lane_follow_client = std::make_shared<rclcpp::Node>("lane_follow_behavior_client");
 
         BT::RosNodeParams stateParameters(_change_state_client, "change_state");
         BT::RosNodeParams navigateParameters(_navigation_client, "/navigate");
         BT::RosNodeParams detectionParameters(_detection_subscriber, "/cam/test/detections");
+        BT::RosNodeParams laneFollowParameters(_lane_follow_client, "/lane_follow");
 
         factory.registerNodeType<ChangeStateBehavior>("ChangeState", stateParameters);
         factory.registerNodeType<NavigateBehavior>("Navigate", navigateParameters);
         factory.registerNodeType<FaceDetectionBehavior>("FaceDetection", navigateParameters);
         factory.registerNodeType<AcquireDetection>("AcquireDetection", detectionParameters);
+        factory.registerNodeType<LaneFollowBehavior>("LaneFollow", laneFollowParameters);
 
         _tree = std::make_unique<BT::Tree>(factory.createTreeFromFile("./src/guppy_tasks/resource/" + std::string(TREE_NAME) + ".xml"));
 
@@ -65,6 +71,7 @@ class NavigationBehaviorTree : public rclcpp::Node {
     std::shared_ptr<rclcpp::Node> _change_state_client;
     std::shared_ptr<rclcpp::Node> _navigation_client;
     std::shared_ptr<rclcpp::Node> _detection_subscriber;
+    std::shared_ptr<rclcpp::Node> _lane_follow_client;
 
     rclcpp::Subscription<guppy_msgs::msg::State>::SharedPtr _subscription;
     rclcpp::TimerBase::SharedPtr _timer;