Markprime1

src/subjugator/mission_planner/CMakeLists.txt

@@ -32,7 +32,8 @@ add_library(
   subjugator_operations/src/poles_big_enough.cpp
   subjugator_operations/src/determine_channel_side.cpp
   subjugator_operations/src/any_poles_detected.cpp
-  subjugator_operations/src/has_found_pair.cpp)
+  subjugator_operations/src/has_found_pair.cpp
+  subjugator_operations/src/waypoint_recorder.cpp)
 
 ament_target_dependencies(
   operations

src/subjugator/mission_planner/include/context.hpp

@@ -1,10 +1,12 @@
 #pragma once
 #include <mutex>
 #include <optional>
+#include <vector>
 
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <yolo_msgs/msg/detection_array.hpp>
@@ -33,6 +35,11 @@ struct Context
     uint32_t img_width{ 0 };
     uint32_t img_height{ 0 };
 
+    // Waypoints
+    std::mutex waypoints_mx;
+    std::vector<geometry_msgs::msg::PoseStamped> waypoints;
+    std::optional<geometry_msgs::msg::PoseStamped> last_waypoint;
+
     inline rclcpp::Logger logger() const
     {
         return node->get_logger();

src/subjugator/mission_planner/src/mission_planner_node.cpp

@@ -18,6 +18,7 @@
 #include "poles_big_enough.hpp"
 #include "publish_goal.hpp"
 #include "track_largest_poles.hpp"
+#include "waypoint_recorder.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <count_when_ticked.hpp>
@@ -91,6 +92,7 @@ int main(int argc, char** argv)
     factory.registerNodeType<DetermineChannelSide>("DetermineChannelSide");
     factory.registerNodeType<AnyPolesDetected>("AnyPolesDetected");
     factory.registerNodeType<HasFoundPair>("HasFoundPair");
+    factory.registerNodeType<WaypointRecorder>("WaypointRecorder");
 
     factory.registerNodeType<TopicTicker<nav_msgs::msg::Odometry>>("TopicTicker");
     factory.registerNodeType<CountWhenTicked>("CountWhenTicked");

src/subjugator/mission_planner/subjugator_missions/xml/relative_move.xml

@@ -13,6 +13,13 @@
                 <RetryUntilSuccessful num_attempts="-1">
                     <Sequence>
                         <Delay delay_msec="50"><AlwaysSuccess/></Delay>
+
+                         <Action ID="WaypointRecorder"
+                        spacing_m="0.5"
+                        x="{abs_x}" y="{abs_y}" z="{abs_z}"
+                        qx="{abs_qx}" qy="{abs_qy}" qz="{abs_qz}" qw="{abs_qw}"
+                        ctx="{ctx}"/>
+
                         <Condition ID="AtGoalPose"
                         x="{abs_x}" y="{abs_y}" z="{abs_z}"
                         qx="{abs_qx}" qy="{abs_qy}" qz="{abs_qz}" qw="{abs_qw}"

src/subjugator/mission_planner/subjugator_operations/include/waypoint_recorder.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+#include <memory>
+
+#include "context.hpp"
+
+struct Context;
+class OperationBase;
+
+class WaypointRecorder final : public BT::SyncActionNode, public OperationBase
+{
+  public:
+    WaypointRecorder(std::string const& name, const BT::NodeConfiguration& config)
+      : BT::SyncActionNode(name, config), OperationBase(config)
+    {
+    }
+
+    static BT::PortsList providedPorts();
+
+    BT::NodeStatus tick() override;
+};

src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp

@@ -0,0 +1,90 @@
+#include "waypoint_recorder.hpp"
+
+#include <behaviortree_cpp/action_node.h>
+#include <behaviortree_cpp/bt_factory.h>
+
+#include <cmath>
+#include <mutex>
+#include <optional>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "context.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "operations.hpp"
+
+BT::NodeStatus WaypointRecorder::tick()
+{
+    double gx, gy, gz, gqx, gqy, gqz, gqw, spacing_m;
+    std::shared_ptr<Context> ctx_;
+
+    if (!getInput("x", gx) || !getInput("y", gy) || !getInput("z", gz) || !getInput("qx", gqx) ||
+        !getInput("qy", gqy) || !getInput("qz", gqz) || !getInput("qw", gqw) || !getInput("spacing_m", spacing_m) ||
+        !getInput("ctx_", ctx_))
+    {
+        RCLCPP_ERROR(ctx_->logger(), "WaypointRecorder: missing required inputs.");
+    }
+
+    std::optional<nav_msgs::msg::Odometry> odom;
+    {
+        std::scoped_lock lk(ctx_->odom_mx);
+        odom = ctx_->latest_odom;
+    }
+
+    if (!odom)
+    {
+        RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 1000, "WaypointRecorder: no odometry yet.");
+    }
+    else
+    {
+        geometry_msgs::msg::PoseStamped wp;
+        wp.header = odom->header;
+        wp.pose = odom->pose.pose;
+
+        std::scoped_lock lkwp(ctx_->waypoints_mx);
+
+        if (!ctx_->last_waypoint)
+        {
+            ctx_->waypoints.push_back(wp);
+            ctx_->last_waypoint = wp;
+
+            if (ctx_)
+            {
+                RCLCPP_INFO(ctx_->logger(), "WaypointRecorder: first waypoint recorded.");
+            }
+        }
+        else
+        {
+            auto const& last = ctx_->last_waypoint->pose.position;
+            double const dx = wp.pose.position.x - last.x;
+            double const dy = wp.pose.position.y - last.y;
+            double const d = std::sqrt(dx * dx + dy * dy);
+
+            if (d >= spacing_m)
+            {
+                ctx_->waypoints.push_back(wp);
+                ctx_->last_waypoint = wp;
+                RCLCPP_INFO(ctx_->logger(), "WaypointRecorder: recorded waypoint #%zu", ctx_->waypoints.size());
+            }
+        }
+    }
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::PortsList WaypointRecorder::providedPorts()
+{
+    BT::PortsList ports;
+    ports.insert(BT::InputPort<double>("x"));
+    ports.insert(BT::InputPort<double>("y"));
+    ports.insert(BT::InputPort<double>("z"));
+    ports.insert(BT::InputPort<double>("qx"));
+    ports.insert(BT::InputPort<double>("qy"));
+    ports.insert(BT::InputPort<double>("qz"));
+    ports.insert(BT::InputPort<double>("qw"));
+    ports.insert(BT::InputPort<double>("spacing_m", 0.5, "Waypoint spacing in meters"));
+    ports.insert(BT::InputPort<std::shared_ptr<Context>>("ctx_"));
+    return ports;
+}