action server set up

Author: MyCabbages4

teleoperation/lander_align/lander_align.hpp

@@ -71,7 +71,7 @@ namespace mrover {
         double const mAngleP = 1;
         double const mAngleFloor = 0.05;
         double const mLinearP = 0.3;
-        
+
         rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr mDebugVectorPub;
 
         //Action Server Variables

teleoperation/lander_align/lander_align.new.cpp

@@ -1 +1,51 @@
-#include "lander_align.new.hpp"
+#include "lander_align.new.hpp"
+
+//LanderAlignActionServer member functions:
+namespace mrover{
+
+    using LanderAlign = action::LanderAlign;
+    using GoalHandleLanderAlign = rclcpp_action::ServerGoalHandle<LanderAlign>;
+
+    LanderAlignNode::LanderAlignNode(const rclcpp::NodeOptions& options) 
+        : Node("lander_align", options) {
+        //The constructor also instantiates a new action server
+        action_server_ = rclcpp_action::create_server<LanderAlign>(
+            this,
+            "lander_align_action",
+            std::bind(&LanderAlignNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+            std::bind(&LanderAlignNode::handle_cancel, this, std::placeholders::_1),
+            std::bind(&LanderAlignNode::handle_accepted, this, std::placeholders::_1));
+    }
+
+    //the callback for handling new goals (this implementation just accepts all goals)
+    rclcpp_action::GoalResponse LanderAlignNode::handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const LanderAlign::Goal> goal) {
+        RCLCPP_INFO(get_logger(), "Received goal request with order %d", goal->num);
+        (void)uuid;
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    //the callback for dealing with cancellation (this implementation just tells the client that it accepted the cancellation)
+    rclcpp_action::CancelResponse LanderAlignNode::handle_cancel(const std::shared_ptr<GoalHandleLanderAlign> goal_handle) {
+        RCLCPP_INFO(get_logger(), "Received request to cancel goal");
+        (void)goal_handle;
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    //the callback for accepting a new goal and processing it
+    void LanderAlignNode::handle_accepted(const std::shared_ptr<GoalHandleLanderAlign> goal_handle) {
+        using namespace std::placeholders;
+        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+        std::thread{std::bind(&LanderAlignNode::execute, this, _1), goal_handle}.detach();
+    }
+
+    //all further processing and updates are done in the execute method in the new thread
+    void LanderAlignNode::execute(const std::shared_ptr<GoalHandleLanderAlign> goal_handle) {
+        RCLCPP_INFO(this->get_logger(), "Executing goal");
+        auto result = std::make_shared<LanderAlign::Result>();
+        // Check if goal is done
+        if (rclcpp::ok()) {
+            goal_handle->succeed(result);
+            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+        }
+    }
+}

teleoperation/lander_align/lander_align.new.hpp

@@ -1,16 +1,43 @@
 #include "pch.hpp"
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <rclcpp/node.hpp>
-#include <mrover/srv/align_lander.hpp>
-
+#include <std_msgs/msg/detail/float32__struct.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <rclcpp/logging.hpp>
+#include <mrover/action/lander_align.hpp>
 namespace mrover {
     class LanderAlignNode final : public rclcpp::Node{
+        public:
+        using LanderAlign = action::LanderAlign;
+        using GoalHandleLanderAlign = rclcpp_action::ServerGoalHandle<LanderAlign>;
+
+        explicit LanderAlignNode(rclcpp::NodeOptions const& options = rclcpp::NodeOptions());
+
+        ~LanderAlignNode() override = default;
+
+        void filterNormals();
+
         private:
-        rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr mCostMapPub;
+        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr mCostMapPub;
         rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr mPcSub;
-        rclcpp::Service<mrover::srv::AlignLander>::SharedPtr mAlignServer;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mPCDebugPub;
+
         geometry_msgs::msg::Pose mFinalAngle;
 
+        rclcpp_action::Server<LanderAlign>::SharedPtr action_server_;
+
+        //the callback for handling new goals (this implementation just accepts all goals)
+        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const LanderAlign::Goal> goal);
+
+        //the callback for dealing with cancellation (this implementation just tells the client that it accepted the cancellation)
+        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleLanderAlign> goal_handle);
+
+        //the callback for accepting a new goal and processing it
+        void handle_accepted(const std::shared_ptr<GoalHandleLanderAlign> goal_handle);
+
+        //all further processing and updates are done in the execute method in the new thread
+        void execute(const std::shared_ptr<GoalHandleLanderAlign> goal_handle);
+
         /*
         Using PCA to calculate plane:
         1. Initial filtering of points by removing any normal with z-component above some threshold (plane normals will be
@@ -21,15 +48,5 @@ namespace mrover {
         4. Do PCA and find normal
         5. Keep doing to find better planes?
         */
-
-        explicit LanderAlignNode(rclcpp::NodeOptions const& options = rclcpp::NodeOptions());
-
-        ~LanderAlignNode() override = default;
-
-        public:
-        
-
-
-        auto alignLanderCallBack(mrover::srv::AlignLander::Request::ConstSharedPtr& req, mrover::srv::AlignLander::Response::SharedPtr& res) -> void;
     };
 }