clean up mission planner

src/subjugator/mission_planner/src/mission_planner_node.cpp

@@ -1,3 +1,4 @@
+#include <behaviortree_cpp/bt_factory.h>
 #include <behaviortree_cpp/loggers/bt_cout_logger.h>
 #include <behaviortree_cpp/loggers/groot2_publisher.h>
 #include <behaviortree_cpp/xml_parsing.h>
@@ -6,16 +7,36 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include "actuate_servo.hpp"
+#include "align_depth.hpp"
+#include "align_yaw.hpp"
+#include "any_poles_detected.hpp"
+#include "at_goal_pose.hpp"
+#include "check_yolo_model.hpp"
 #include "context.hpp"
-#include "start_coin_flip.hpp"
+#include "detect_target.hpp"
+#include "determine_channel_side.hpp"
+#include "has_found_pair.hpp"
+#include "hone_bearing.hpp"
+#include "hone_midpoint.hpp"
+#include "log_to_file.hpp"
+#include "nav_channel_control.hpp"
+#include "poles_big_enough.hpp"
+#include "publish_goal.hpp"
 #include "std_srvs/srv/set_bool.hpp"
 #include "subjugator_msgs/msg/thruster_efforts.hpp"
+#include "track_best_pair.hpp"
+#include "track_largest_poles.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
+#include <count_when_ticked.hpp>
+#include <go_to_pinger.hpp>
+#include <pitch_style.hpp>
+#include <roll_style.hpp>
+#include <topic_ticker.hpp>
+#include <yaw_style.hpp>
 #include <yolo_msgs/msg/detection_array.hpp>
 
-BT::BehaviorTreeFactory factory;
-
 int main(int argc, char** argv)
 {
     rclcpp::init(argc, argv);
@@ -26,11 +47,6 @@ int main(int argc, char** argv)
     node->declare_parameter<std::string>("mission", "SonarFollowerTest");
     std::string mission_to_run = node->get_parameter("mission").as_string();
 
-    node->declare_parameter<std::string>("detections_topic", "/yolo/detections");
-    node->declare_parameter<std::string>("tracking_topic", "/yolo/tracking");
-    std::string detections_topic = node->get_parameter("detections_topic").as_string();
-    std::string tracking_topic = node->get_parameter("tracking_topic").as_string();
-
     // Topics to subscribe/publish to
     ctx->goal_pub = node->create_publisher<geometry_msgs::msg::Pose>("/goal_pose", 10);
     ctx->odom_sub = node->create_subscription<nav_msgs::msg::Odometry>("/odometry/filtered", 10,
@@ -40,31 +56,14 @@ int main(int argc, char** argv)
                                                                            ctx->latest_odom = *msg;
                                                                        });
 
-    RCLCPP_INFO(node->get_logger(), "Subscribing to YOLO detections on '%s' and tracking on '%s'",
-                detections_topic.c_str(), tracking_topic.c_str());
-    ctx->detections_sub =
-        node->create_subscription<yolo_msgs::msg::DetectionArray>(detections_topic, 10,
+    // Perception targets: from your YOLO node
+    ctx->targets_sub =
+        node->create_subscription<yolo_msgs::msg::DetectionArray>("/yolo/detections", 10,
                                                                   [ctx](yolo_msgs::msg::DetectionArray::SharedPtr msg)
                                                                   {
                                                                       std::scoped_lock lk(ctx->detections_mx);
                                                                       ctx->latest_detections = *msg;
                                                                   });
-    ctx->tracking_sub =
-        node->create_subscription<yolo_msgs::msg::DetectionArray>(tracking_topic, 10,
-                                                                  [ctx](yolo_msgs::msg::DetectionArray::SharedPtr msg)
-                                                                  {
-                                                                      std::scoped_lock lk(ctx->tracking_mx);
-                                                                      ctx->latest_tracking = *msg;
-                                                                  });
-
-    // Wall orientation from the coin_flip classifier node (subjugator_vision)
-    ctx->wall_direction_sub =
-        node->create_subscription<std_msgs::msg::String>("/coin_flip/direction", 10,
-                                                         [ctx](std_msgs::msg::String::SharedPtr msg)
-                                                         {
-                                                             std::scoped_lock lk(ctx->wall_direction_mx);
-                                                             ctx->latest_wall_direction = msg->data;
-                                                         });
 
     // Image size (for pixel->angle mapping). Probably do not need
     ctx->image_sub = node->create_subscription<sensor_msgs::msg::Image>("/front_cam/image_raw", 10,
@@ -82,37 +81,50 @@ int main(int argc, char** argv)
     ctx->controller_enable_client = node->create_client<std_srvs::srv::SetBool>("/pid_controller/enable", 10);
     ctx->raw_effort_pub = node->create_publisher<subjugator_msgs::msg::ThrusterEfforts>("/thruster_efforts", 10);
 
-    // Wait for odometry before starting mission
-    RCLCPP_INFO(node->get_logger(), "Waiting for odometry...");
-    rclcpp::Rate wait_rate(10.0);
-    while (rclcpp::ok())
+    BT::BehaviorTreeFactory factory;
+    factory.registerNodeType<PublishGoalPose>("PublishGoalPose");
+    factory.registerNodeType<AtGoalPose>("AtGoalPose");
+    factory.registerNodeType<LogToFile>("LogToFile");
+    factory.registerNodeType<DetectTarget>("DetectTarget");
+    factory.registerNodeType<HoneBearing>("HoneBearing");
+    factory.registerNodeType<CheckYoloModel>("CheckYoloModel");
+    factory.registerNodeType<TrackLargestPoles>("TrackLargestPoles");
+    factory.registerNodeType<PolesBigEnough>("PolesBigEnough");
+    factory.registerNodeType<DetermineChannelSide>("DetermineChannelSide");
+    factory.registerNodeType<AnyPolesDetected>("AnyPolesDetected");
+    factory.registerNodeType<HasFoundPair>("HasFoundPair");
+    factory.registerNodeType<TrackBestPair>("TrackBestPair");
+    factory.registerNodeType<HoneMidpoint>("HoneMidpoint");
+    factory.registerNodeType<NavChannelControl>("NavChannelControl");
+    factory.registerNodeType<ActuateServo>("ActuateServo");
+    factory.registerNodeType<AlignDepth>("AlignDepth");
+    factory.registerNodeType<AlignYaw>("AlignYaw");
+
+    factory.registerNodeType<TopicTicker<nav_msgs::msg::Odometry>>("TopicTicker");
+    factory.registerNodeType<CountWhenTicked>("CountWhenTicked");
+    factory.registerNodeType<SonarFollower>("SonarFollower");
+    factory.registerNodeType<YawStyle>("YawStyle");
+    factory.registerNodeType<RollStyle>("RollStyle");
+    factory.registerNodeType<PitchStyle>("PitchStyle");
+
+    // Load all tree models from installed xml
+    std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner");
+    for (auto const& entry : std::filesystem::directory_iterator(std::filesystem::path(pkg_share) / "bt"))
     {
-        rclcpp::spin_some(node);
+        if (entry.path().extension() == ".xml")
         {
-            std::scoped_lock lk(ctx->odom_mx);
-            if (ctx->latest_odom.has_value())
-            {
-                RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!");
-                break;
-            }
+            factory.registerBehaviorTreeFromFile(entry.path().string());
         }
-        wait_rate.sleep();
     }
 
-    // Load all tree models from installed xml
-    std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner");
-    std::string const bt_dir = (std::filesystem::path(pkg_share) / "bt").string();
-    auto bt_path = [&](std::string const& file) { return (std::filesystem::path(bt_dir) / file).string(); };
-    factory.registerBehaviorTreeFromFile(bt_path("sub9_missions.xml"));
-
     // Create by name
     auto blackboard = BT::Blackboard::create();
     blackboard->set("ctx", ctx);
 
-    std::unique_ptr<BT::Tree> tree_ptr;
+    BT::Tree tree;
     try
     {
-        tree_ptr = std::make_unique<BT::Tree>(factory.createTree(mission_to_run, blackboard));
+        tree = factory.createTree(mission_to_run, blackboard);
     }
     catch (std::exception const& e)
     {
@@ -121,37 +133,50 @@ int main(int argc, char** argv)
         return 1;
     }
 
+    // Wait for odometry before starting mission
+    RCLCPP_INFO(node->get_logger(), "Waiting for odometry...");
+    rclcpp::Rate wait_rate(10.0);
+    while (rclcpp::ok())
+    {
+        rclcpp::spin_some(node);
+        {
+            std::scoped_lock lk(ctx->odom_mx);
+            if (ctx->latest_odom.has_value())
+            {
+                RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!");
+                break;
+            }
+        }
+        wait_rate.sleep();
+    }
+
     // For live feed of tree
-    BT::Groot2Publisher publisher(*tree_ptr);
+    BT::Groot2Publisher publisher(tree);
 
     // Log BT transitions to console
-    BT::StdCoutLogger logger_cout(*tree_ptr);
+    BT::StdCoutLogger logger_cout(tree);
 
     RCLCPP_INFO(node->get_logger(), "Mission Planner started. Ticking tree…");
     rclcpp::WallRate rate(30.0);
 
     std::string xml_models = BT::writeTreeNodesModelXML(factory);
-    std::ofstream("/home/carlos/models.xml") << xml_models;
+    std::ofstream("/tmp/models.xml") << xml_models;
 
     while (rclcpp::ok())
     {
         rclcpp::spin_some(node);
-        BT::NodeStatus status = tree_ptr->tickOnce();
+        BT::NodeStatus status = tree.tickOnce();
 
         if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE)
         {
             RCLCPP_INFO(node->get_logger(), "Mission finished with status: %s. Shutting down.",
                         (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE"));
-            tree_ptr->haltTree();
+            tree.haltTree();
             break;
         }
         rate.sleep();
     }
 
-    // Kill the coin_flip_node if the mission launched it. Runs on every exit
-    // path, including Ctrl-C, since the loop ends when ok() is false.
-    stopCoinFlip(*ctx);
-
     rclcpp::shutdown();
     return 0;
 }

src/subjugator/mission_planner/subjugator_missions/xml/absolute_move.xml

@@ -1,4 +1,4 @@
-<root main_tree_to_execute="AbsoluteMovetionMission">
+<root BTCPP_format="4" main_tree_to_execute="AbsoluteMovetionMission">
     <BehaviorTree ID="AbsoluteMovetionMission">
         <Sequence>
             <Action ID="PublishGoalPose"

src/subjugator/mission_planner/subjugator_missions/xml/sonar_follower_test_mission.xml

@@ -1,4 +1,4 @@
-<root main_tree_to_execute="SonarFollowerTest">
+<root BTCPP_format="4" main_tree_to_execute="SonarFollowerTest">
     <BehaviorTree ID="SonarFollowerTest">
         <SonarFollower name="sonar_follower" ctx="{ctx}" sonar_x="{sonar_x}" sonar_y="{sonar_y}" sonar_z="{sonar_z}" sonar_w="{sonar_w}">
                 <SubTree ID="RelativeMove" _autoremap="true"

src/subjugator/mission_planner/subjugator_missions/xml/topic_ticker_test_mission.xml

@@ -1,4 +1,4 @@
-<root main_tree_to_execute="TopicTickerTest">
+<root BTCPP_format="4" main_tree_to_execute="TopicTickerTest">
   <BehaviorTree ID="TopicTickerTest">
     <TopicTickerOdom name="topic_ticker" topic_name="dvl/odom" ctx="{ctx}">
       <CountWhenTicked name="counter_action"/>