Carlos image centralizing

src/subjugator/mission_planner/CMakeLists.txt

@@ -27,6 +27,7 @@ add_library(
   subjugator_operations/src/at_goal_pose.cpp
   subjugator_operations/src/detect_target.cpp
   subjugator_operations/src/hone_bearing.cpp
+  subjugator_operations/src/yaw_p_controller.cpp
   subjugator_operations/src/check_yolo_model.cpp
   subjugator_operations/src/track_largest_poles.cpp
   subjugator_operations/src/poles_big_enough.cpp

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 "yaw_p_controller.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <count_when_ticked.hpp>
@@ -96,6 +97,7 @@ int main(int argc, char** argv)
     factory.registerNodeType<CountWhenTicked>("CountWhenTicked");
     factory.registerNodeType<SonarFollower>("SonarFollower");
     factory.registerNodeType<YawStyle>("YawStyle");
+    factory.registerNodeType<YawPController>("YawPController");
 
     // Load all tree models from installed xml
     std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner");

src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing.xml

@@ -0,0 +1,26 @@
+<root BTCPP_format="4">
+    <BehaviorTree ID="HoneBearingMove">
+        <Sequence>
+            <Action ID="HoneBearing"
+                    label="{label}"
+                    min_conf="{min_conf}"
+                    offset_deg="{offset_deg}"
+                    fov_deg="{fov_deg}"
+                    method="{method}"
+                    ctx="{ctx}"
+                    found="{hb_found}"
+                    bearing_deg="{hb_bearing_deg}"
+                    error_deg="{hb_error_deg}"
+                    yaw_cmd_deg="{hb_yaw_cmd_deg}"
+                    best_cx_px="{hb_best_cx_px}"
+                    best_conf="{hb_best_conf}" />
+
+            <SubTree ID="RelativeMove" _autoremap="true"
+                        x="0.0" y="0.0" z="0.0"
+                        roll_deg="0.0" pitch_deg="0.0" yaw_deg="{hb_yaw_cmd_deg}"
+                        pos_tol="{pos_tol}" ori_tol_deg="{ori_tol_deg}"
+                        msec="{msec}"
+                        ctx="{ctx}" />
+        </Sequence>
+    </BehaviorTree>
+</root>

src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing_test_mission.xml

@@ -0,0 +1,17 @@
+<root BTCPP_format="4" main_tree_to_execute="HoneBearingTestMission">
+  <BehaviorTree ID="HoneBearingTestMission">
+    <Sequence>
+      <SubTree ID="HoneBearingMove" _autoremap="true"
+               label="shark"
+               min_conf="0.30"
+               offset_deg="0.0"
+               fov_deg="65.8"
+               method="atan"
+               tol_deg="2.0"
+               pos_tol="0.25"
+               ori_tol_deg="2.0"
+               msec="10000"
+               ctx="{ctx}" />
+    </Sequence>
+  </BehaviorTree>
+</root>

src/subjugator/mission_planner/subjugator_missions/xml/hone_p_controller.xml

@@ -0,0 +1,37 @@
+<root BTCPP_format="4" main_tree_to_execute="HonePController">
+
+  <BehaviorTree ID="HonePController">
+    <Timeout msec="30000">
+      <!-- Succeed when controller succeeds & fail if movement fails -->
+      <Parallel success_count="1" failure_count="1">
+
+        <!-- Controller outputs yaw_cmd -->
+        <Action ID="YawPController"
+                label="shark"
+                min_conf="0.30"
+                kp="0.02"
+                tol_px="8.0"
+                hold_ticks="10"
+                max_cmd="10.0"
+                ctx="{ctx}"
+                centered="{centered}"
+                best_cx_px="{best_cx_px}"
+                best_conf="{best_conf}"
+                yaw_cmd="{yaw_cmd}"/>
+
+        <!-- Keep applying small relative yaw moves using the latest yaw_cmd -->
+        <Repeat num_cycles="-1">
+          <SubTree ID="RelativeMove" _autoremap="true"
+                   x="0.0" y="0.0" z="0.0"
+                   yaw_deg="{yaw_cmd}"
+                   pos_tol="0.20"
+                   ori_tol_deg="5.0"
+                   msec="3000"
+                   ctx="{ctx}"/>
+        </Repeat>
+
+      </Parallel>
+    </Timeout>
+  </BehaviorTree>
+
+</root>

src/subjugator/mission_planner/subjugator_missions/xml/nav_channel_mission.xml

@@ -15,7 +15,7 @@
                 <ForceSuccess>
                     <Timeout msec="35000">
                         <Action ID="TrackLargestPoles"
-                                min_conf="0.20" fov_deg="110.0" require_red_left="false"
+                                min_conf="0.20" fov_deg="64.18" require_red_left="false"
                                 reset_on_start="true" ctx="{ctx}"
                                 best_qx="{best_qx}" best_qy="{best_qy}" best_qz="{best_qz}" best_qw="{best_qw}"
                                 best_target_qx="{best_target_qx}" best_target_qy="{best_target_qy}"
@@ -125,7 +125,7 @@
                             <Fallback>
                                 <Timeout msec="35000">
                                     <Action ID="TrackLargestPoles"
-                                        min_conf="0.30" fov_deg="110.0" require_red_left="{require_red_left}"
+                                        min_conf="0.30" fov_deg="64.18" require_red_left="{require_red_left}"
                                         reset_on_start="true" ctx="{ctx}"
                                         best_qx="{best_qx}" best_qy="{best_qy}" best_qz="{best_qz}" best_qw="{best_qw}"
                                         best_target_qx="{best_target_qx}" best_target_qy="{best_target_qy}"
@@ -144,23 +144,23 @@
                                 <Repeat num_cycles="2">
                                     <Fallback>
                                         <SubTree ID="RelativeMove" _autoremap="true"
-                                            yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3500" ctx="{ctx}"/>
+                                            yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/>
                                         <AlwaysSuccess/>
                                     </Fallback>
                                 </Repeat>
                                 <!-- to +30 degrees total -->
                                 <Repeat num_cycles="4">
                                     <Fallback>
                                         <SubTree ID="RelativeMove" _autoremap="true"
-                                            yaw_deg="15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3500" ctx="{ctx}"/>
+                                            yaw_deg="15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/>
                                         <AlwaysSuccess/>
                                     </Fallback>
                                 </Repeat>
                                 <!-- back to center -->
                                 <Repeat num_cycles="2">
                                     <Fallback>
                                         <SubTree ID="RelativeMove" _autoremap="true"
-                                            yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3500" ctx="{ctx}"/>
+                                            yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/>
                                         <AlwaysSuccess/>
                                     </Fallback>
                                 </Repeat>

src/subjugator/mission_planner/subjugator_missions/xml/start_gate_mission.xml

@@ -58,17 +58,24 @@
 
       </Parallel>
 
+      <Delay delay_msec="1000"><AlwaysSuccess/></Delay>
       <!-- 2) Hone orientation toward the shark with offset -->
-      <Action ID="HoneBearing"
-              label="shark"
-              offset_deg="0.0"
-              fov_deg="110.0"
-              min_conf="0.40"
-              ctx="{ctx}"/>
+      <SubTree ID="HoneBearingMove" _autoremap="true"
+               label="shark"
+               min_conf="0.30"
+               offset_deg="0.0"
+               fov_deg="65.8"
+               method="atan"
+               tol_deg="2.0"
+               pos_tol="0.25"
+               ori_tol_deg="2.0"
+               msec="10000"
+               ctx="{ctx}"/>
+
 
       <!-- 3) Then down and forward -->
       <SubTree ID="RelativeMove" _autoremap="true"
-               z="-0.5"
+               z="-0.1"
                pos_tol="0.25" ori_tol_deg="12.0" msec="10000"
                ctx="{ctx}"/>
       <SubTree ID="RelativeMove" _autoremap="true"

src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml

@@ -13,4 +13,7 @@
     <include path="topic_ticker_test_mission.xml"/>
     <include path="sonar_follower_test_mission.xml"/>
     <include path="yaw_style.xml"/>
+    <include path="hone_bearing.xml"/>
+    <include path="hone_bearing_test_mission.xml"/>
+    <include path="hone_p_controller.xml"/>
 </root>

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

@@ -3,10 +3,13 @@
 #include <behaviortree_cpp/bt_factory.h>
 
 #include <memory>
+#include <optional>
 #include <string>
 
 #include "context.hpp"
 
+#include <yolo_msgs/msg/detection_array.hpp>
+
 class HoneBearing : public BT::StatefulActionNode
 {
   public:
@@ -18,6 +21,20 @@ class HoneBearing : public BT::StatefulActionNode
     void onHalted() override;
 
   private:
+    static double deg2rad(double deg);
+    static double rad2deg(double rad);
+
+    struct BestDet
+    {
+        double cx_px{ 0.0 };
+        double conf{ 0.0 };
+    };
+
+    static std::optional<BestDet> findBestDet_(yolo_msgs::msg::DetectionArray const& arr, std::string const& label,
+                                               double min_conf);
+
+    static std::optional<double> bearingLinearDeg_(double cx_px, uint32_t width_px, double hfov_deg);
+    static std::optional<double> bearingAtanDeg_(double cx_px, uint32_t width_px, double hfov_deg);
+
     std::shared_ptr<Context> ctx_;
-    bool published_{ false };
 };

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

@@ -0,0 +1,59 @@
+// yaw_p_controller.hpp
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "context.hpp"
+
+#include <yolo_msgs/msg/detection_array.hpp>
+
+/**
+ * YawPController
+ *
+ * Vision-based yaw centering using a P controller on pixel error.
+ *
+ * - Each tick: reads latest YOLO detections + image width, computes pixel error vs image center,
+ *   outputs yaw_cmd = clamp(-kp * error_px, [-max_cmd, +max_cmd]).
+ * - Returns RUNNING until centered for hold_ticks consecutive ticks, then SUCCESS.
+ * - Returns FAILURE on missing ctx / overall timeout
+ */
+class YawPController : public BT::StatefulActionNode
+{
+  public:
+    YawPController(std::string const& name, BT::NodeConfiguration const& cfg);
+
+    static BT::PortsList providedPorts();
+
+    BT::NodeStatus onStart() override;
+    BT::NodeStatus onRunning() override;
+    void onHalted() override;
+
+  private:
+    struct BestDet
+    {
+        double cx_px{ 0.0 };
+        double conf{ 0.0 };
+    };
+
+    std::optional<BestDet> findBestDet_(yolo_msgs::msg::DetectionArray const& arr, std::string const& label,
+                                        double min_conf) const;
+
+    static double nowSec_(rclcpp::Node const* node);
+
+  private:
+    // Shared context from input port "ctx"
+    std::shared_ptr<Context> ctx_{};
+
+    // Stateful bookkeeping across ticks
+    int stable_count_{ 0 };
+
+    // Timing (seconds)
+    double start_t_{ 0.0 };
+    double last_det_t_{ 0.0 };
+};