From 28bae822f1f88a7d2e26f4a20378ea11cf481e50 Mon Sep 17 00:00:00 2001 From: markprime13 Date: Wed, 18 Feb 2026 13:23:31 -0500 Subject: [PATCH 1/6] test change --- .../subjugator_operations/src/any_poles_detected.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp index bfea70543..915abab29 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp @@ -14,7 +14,7 @@ BT::NodeStatus AnyPolesDetected::tick() { if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) return BT::NodeStatus::FAILURE; - double min_conf = 0.30; + double min_conf = 0.31; (void)getInput("min_conf", min_conf); std::optional arr; From e881d9a4021245ebb545db88da4f17d60e0b9940 Mon Sep 17 00:00:00 2001 From: markprime13 Date: Wed, 18 Feb 2026 13:24:29 -0500 Subject: [PATCH 2/6] test change 1 --- .../subjugator_operations/src/any_poles_detected.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp index 915abab29..bfea70543 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp @@ -14,7 +14,7 @@ BT::NodeStatus AnyPolesDetected::tick() { if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) return BT::NodeStatus::FAILURE; - double min_conf = 0.31; + double min_conf = 0.30; (void)getInput("min_conf", min_conf); std::optional arr; From e840a0057fdf8d7c65d8063e0a36bd1b847d9264 Mon Sep 17 00:00:00 2001 From: markprime13 Date: Wed, 18 Feb 2026 13:26:52 -0500 Subject: [PATCH 3/6] test change 2 --- .../subjugator_operations/src/any_poles_detected.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp index bfea70543..6631a8bea 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp @@ -15,6 +15,7 @@ BT::NodeStatus AnyPolesDetected::tick() if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) return BT::NodeStatus::FAILURE; double min_conf = 0.30; + double min_conf1 = 0.31; (void)getInput("min_conf", min_conf); std::optional arr; From a420b7ad2dfe135403c95039d7adbf63421572d3 Mon Sep 17 00:00:00 2001 From: markprime13 Date: Wed, 18 Feb 2026 14:15:48 -0500 Subject: [PATCH 4/6] waypoint recorder draft --- src/subjugator/mission_planner/CMakeLists.txt | 3 +- .../src/any_poles_detected.cpp | 1 - .../src/waypoint_recorder.cpp | 102 ++++++++++++++++++ 3 files changed, 104 insertions(+), 2 deletions(-) create mode 100644 src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 4e1527cbe..0a592d81f 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/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 diff --git a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp index 6631a8bea..bfea70543 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/any_poles_detected.cpp @@ -15,7 +15,6 @@ BT::NodeStatus AnyPolesDetected::tick() if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) return BT::NodeStatus::FAILURE; double min_conf = 0.30; - double min_conf1 = 0.31; (void)getInput("min_conf", min_conf); std::optional arr; diff --git a/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp new file mode 100644 index 000000000..d16ad3bb5 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp @@ -0,0 +1,102 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" + +class WaypointRecorder : public rclcpp::Node +{ + public: + WaypointRecorder() + : Node("waypoint_recorder") + , spacing_m_(declare_parameter("spacing_m", 0.5)) + , odom_topic_(declare_parameter("odom_topic", "/odometry/filtered")) + , path_topic_(declare_parameter("path_topic", "/recorded_path")) + , have_last_(false) + { + using std::placeholders::_1; + + path_pub_ = create_publisher(path_topic_, rclcpp::QoS(1).transient_local()); + + odom_sub_ = create_subscription(odom_topic_, rclcpp::QoS(50), + std::bind(&WaypointRecorder::odomCb, this, _1)); + + RCLCPP_INFO(get_logger(), "WaypointRecorder started. odom_topic='%s', spacing=%.2fm, path_topic='%s'", + odom_topic_.c_str(), spacing_m_, path_topic_.c_str()); + } + + private: + static double dist2D(geometry_msgs::msg::Point const &a, geometry_msgs::msg::Point const &b) + { + double const dx = a.x - b.x; + double const dy = a.y - b.y; + return std::sqrt(dx * dx + dy * dy); + } + + void odomCb(nav_msgs::msg::Odometry::SharedPtr const msg) + { + // Build a PoseStamped waypoint in the same frame/time as the odometry + geometry_msgs::msg::PoseStamped wp; + wp.header = msg->header; // keeps frame_id + stamp + wp.pose = msg->pose.pose; // position + orientation from fused odom + + std::scoped_lock lock(mutex_); + + if (!have_last_) + { + // First waypoint = starting pose + pushWaypointLocked(wp); + last_wp_ = wp; + have_last_ = true; + return; + } + + // Record only when we moved >= spacing_m_ from last recorded waypoint + double const d = dist2D(wp.pose.position, last_wp_.pose.position); + if (d >= spacing_m_) + { + pushWaypointLocked(wp); + last_wp_ = wp; + } + } + + void pushWaypointLocked(geometry_msgs::msg::PoseStamped const &wp) + { + waypoints_.push_back(wp); + + // Publish as nav_msgs/Path (handy for RViz + downstream) + nav_msgs::msg::Path path; + path.header = wp.header; // frame_id matches odom frame + path.poses = waypoints_; + + path_pub_->publish(path); + + RCLCPP_DEBUG(get_logger(), "Recorded waypoint #%zu at (%.2f, %.2f, %.2f)", waypoints_.size(), + wp.pose.position.x, wp.pose.position.y, wp.pose.position.z); + } + + private: + double spacing_m_; + std::string odom_topic_; + std::string path_topic_; + + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + + std::mutex mutex_; + std::vector waypoints_; + geometry_msgs::msg::PoseStamped last_wp_; + bool have_last_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 54cec9fe122d3e625e56e638be7461cdeda5382e Mon Sep 17 00:00:00 2001 From: markprime13 Date: Sat, 21 Feb 2026 17:31:42 -0500 Subject: [PATCH 5/6] waypoint recorder --- .../src/waypoint_recorder.cpp | 108 +++++------------- 1 file changed, 29 insertions(+), 79 deletions(-) diff --git a/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp index d16ad3bb5..7c654180d 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp @@ -8,95 +8,45 @@ #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" +BT::PortsList AtGoalPose::providedPorts() +{ + BT::PortsList ports; + ports.insert(BT::InputPort("x")); + ports.insert(BT::InputPort("y")); + ports.insert(BT::InputPort("z")); + ports.insert(BT::InputPort("qx")); + ports.insert(BT::InputPort("qy")); + ports.insert(BT::InputPort("qz")); + ports.insert(BT::InputPort("qw")); + ports.insert(BT::InputPort("pos_tol", 0.20, "Position tolerance (m)")); + ports.insert(BT::InputPort("ori_tol_deg", 10.0, "Orientation tolerance (deg)")); + ports.insert(BT::InputPort>("ctx")); + return ports; +} + class WaypointRecorder : public rclcpp::Node { - public: - WaypointRecorder() - : Node("waypoint_recorder") - , spacing_m_(declare_parameter("spacing_m", 0.5)) - , odom_topic_(declare_parameter("odom_topic", "/odometry/filtered")) - , path_topic_(declare_parameter("path_topic", "/recorded_path")) - , have_last_(false) + double gx, gy, gz, gqx, gqy, gqz, gqw, pos_tol, ori_tol_deg; + if (!getInput("x", gx) || !getInput("y", gy) || !getInput("z", gz) || !getInput("qx", gqx) || + !getInput("qy", gqy) || !getInput("qz", gqz) || !getInput("qw", gqw) || !getInput("pos_tol", pos_tol) || + !getInput("ori_tol_deg", ori_tol_deg)) { - using std::placeholders::_1; - - path_pub_ = create_publisher(path_topic_, rclcpp::QoS(1).transient_local()); - - odom_sub_ = create_subscription(odom_topic_, rclcpp::QoS(50), - std::bind(&WaypointRecorder::odomCb, this, _1)); - - RCLCPP_INFO(get_logger(), "WaypointRecorder started. odom_topic='%s', spacing=%.2fm, path_topic='%s'", - odom_topic_.c_str(), spacing_m_, path_topic_.c_str()); + RCLCPP_ERROR(ctx_->logger(), "WaypointRecorder: missing required inputs."); } - private: - static double dist2D(geometry_msgs::msg::Point const &a, geometry_msgs::msg::Point const &b) + std::optional odom; { - double const dx = a.x - b.x; - double const dy = a.y - b.y; - return std::sqrt(dx * dx + dy * dy); + std::scoped_lock lk(ctx_->odom_mx); + odom = ctx_->latest_odom; } - void odomCb(nav_msgs::msg::Odometry::SharedPtr const msg) + if (!odom) { - // Build a PoseStamped waypoint in the same frame/time as the odometry - geometry_msgs::msg::PoseStamped wp; - wp.header = msg->header; // keeps frame_id + stamp - wp.pose = msg->pose.pose; // position + orientation from fused odom - - std::scoped_lock lock(mutex_); - - if (!have_last_) - { - // First waypoint = starting pose - pushWaypointLocked(wp); - last_wp_ = wp; - have_last_ = true; - return; - } - - // Record only when we moved >= spacing_m_ from last recorded waypoint - double const d = dist2D(wp.pose.position, last_wp_.pose.position); - if (d >= spacing_m_) - { - pushWaypointLocked(wp); - last_wp_ = wp; - } + RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 1000, "WaypointRecorder: no odometry yet."); } - void pushWaypointLocked(geometry_msgs::msg::PoseStamped const &wp) - { - waypoints_.push_back(wp); + auto const& p = odom->pose.pose.position; + auto const& q = odom->pose.pose.orientation; - // Publish as nav_msgs/Path (handy for RViz + downstream) - nav_msgs::msg::Path path; - path.header = wp.header; // frame_id matches odom frame - path.poses = waypoints_; - - path_pub_->publish(path); - - RCLCPP_DEBUG(get_logger(), "Recorded waypoint #%zu at (%.2f, %.2f, %.2f)", waypoints_.size(), - wp.pose.position.x, wp.pose.position.y, wp.pose.position.z); - } - - private: - double spacing_m_; - std::string odom_topic_; - std::string path_topic_; - - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Publisher::SharedPtr path_pub_; - - std::mutex mutex_; - std::vector waypoints_; - geometry_msgs::msg::PoseStamped last_wp_; - bool have_last_; + // capture the odometry frame here }; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} From 0d10b57108a05a6b660f33089a49b3db2675a0f4 Mon Sep 17 00:00:00 2001 From: markprime13 Date: Fri, 27 Feb 2026 13:05:16 -0500 Subject: [PATCH 6/6] waypoint recorder 1 --- .../mission_planner/include/context.hpp | 7 ++ .../src/mission_planner_node.cpp | 2 + .../subjugator_missions/xml/relative_move.xml | 7 ++ .../include/waypoint_recorder.hpp | 23 +++++ .../src/waypoint_recorder.cpp | 84 ++++++++++++++----- 5 files changed, 100 insertions(+), 23 deletions(-) create mode 100644 src/subjugator/mission_planner/subjugator_operations/include/waypoint_recorder.hpp diff --git a/src/subjugator/mission_planner/include/context.hpp b/src/subjugator/mission_planner/include/context.hpp index a7e4884e9..3469d2425 100644 --- a/src/subjugator/mission_planner/include/context.hpp +++ b/src/subjugator/mission_planner/include/context.hpp @@ -1,10 +1,12 @@ #pragma once #include #include +#include #include #include +#include #include #include #include @@ -33,6 +35,11 @@ struct Context uint32_t img_width{ 0 }; uint32_t img_height{ 0 }; + // Waypoints + std::mutex waypoints_mx; + std::vector waypoints; + std::optional last_waypoint; + inline rclcpp::Logger logger() const { return node->get_logger(); diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index 686bd19bc..e11e753fb 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/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 #include @@ -91,6 +92,7 @@ int main(int argc, char** argv) factory.registerNodeType("DetermineChannelSide"); factory.registerNodeType("AnyPolesDetected"); factory.registerNodeType("HasFoundPair"); + factory.registerNodeType("WaypointRecorder"); factory.registerNodeType>("TopicTicker"); factory.registerNodeType("CountWhenTicked"); diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/relative_move.xml b/src/subjugator/mission_planner/subjugator_missions/xml/relative_move.xml index 43eb78468..1740f70f0 100644 --- a/src/subjugator/mission_planner/subjugator_missions/xml/relative_move.xml +++ b/src/subjugator/mission_planner/subjugator_missions/xml/relative_move.xml @@ -13,6 +13,13 @@ + + + + +#include + +#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; +}; diff --git a/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp index 7c654180d..62eee46b0 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/waypoint_recorder.cpp @@ -1,35 +1,29 @@ +#include "waypoint_recorder.hpp" + +#include +#include + #include #include +#include #include #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::PortsList AtGoalPose::providedPorts() +BT::NodeStatus WaypointRecorder::tick() { - BT::PortsList ports; - ports.insert(BT::InputPort("x")); - ports.insert(BT::InputPort("y")); - ports.insert(BT::InputPort("z")); - ports.insert(BT::InputPort("qx")); - ports.insert(BT::InputPort("qy")); - ports.insert(BT::InputPort("qz")); - ports.insert(BT::InputPort("qw")); - ports.insert(BT::InputPort("pos_tol", 0.20, "Position tolerance (m)")); - ports.insert(BT::InputPort("ori_tol_deg", 10.0, "Orientation tolerance (deg)")); - ports.insert(BT::InputPort>("ctx")); - return ports; -} + double gx, gy, gz, gqx, gqy, gqz, gqw, spacing_m; + std::shared_ptr ctx_; -class WaypointRecorder : public rclcpp::Node -{ - double gx, gy, gz, gqx, gqy, gqz, gqw, pos_tol, ori_tol_deg; if (!getInput("x", gx) || !getInput("y", gy) || !getInput("z", gz) || !getInput("qx", gqx) || - !getInput("qy", gqy) || !getInput("qz", gqz) || !getInput("qw", gqw) || !getInput("pos_tol", pos_tol) || - !getInput("ori_tol_deg", ori_tol_deg)) + !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."); } @@ -44,9 +38,53 @@ class WaypointRecorder : public rclcpp::Node { 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); - auto const& p = odom->pose.pose.position; - auto const& q = odom->pose.pose.orientation; + if (!ctx_->last_waypoint) + { + ctx_->waypoints.push_back(wp); + ctx_->last_waypoint = wp; - // capture the odometry frame here -}; + 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("x")); + ports.insert(BT::InputPort("y")); + ports.insert(BT::InputPort("z")); + ports.insert(BT::InputPort("qx")); + ports.insert(BT::InputPort("qy")); + ports.insert(BT::InputPort("qz")); + ports.insert(BT::InputPort("qw")); + ports.insert(BT::InputPort("spacing_m", 0.5, "Waypoint spacing in meters")); + ports.insert(BT::InputPort>("ctx_")); + return ports; +}