diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 4e1527cb..8bc2ef43 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/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 diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index 686bd19b..0db679da 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 "yaw_p_controller.hpp" #include #include @@ -96,6 +97,7 @@ int main(int argc, char** argv) factory.registerNodeType("CountWhenTicked"); factory.registerNodeType("SonarFollower"); factory.registerNodeType("YawStyle"); + factory.registerNodeType("YawPController"); // Load all tree models from installed xml std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner"); diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing.xml b/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing.xml new file mode 100644 index 00000000..f3437eba --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing.xml @@ -0,0 +1,26 @@ + + + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing_test_mission.xml b/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing_test_mission.xml new file mode 100644 index 00000000..1d969d8d --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/hone_bearing_test_mission.xml @@ -0,0 +1,17 @@ + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/hone_p_controller.xml b/src/subjugator/mission_planner/subjugator_missions/xml/hone_p_controller.xml new file mode 100644 index 00000000..36305ebb --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/hone_p_controller.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/nav_channel_mission.xml b/src/subjugator/mission_planner/subjugator_missions/xml/nav_channel_mission.xml index 43b5fbc0..1a532cfa 100644 --- a/src/subjugator/mission_planner/subjugator_missions/xml/nav_channel_mission.xml +++ b/src/subjugator/mission_planner/subjugator_missions/xml/nav_channel_mission.xml @@ -15,7 +15,7 @@ + yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/> @@ -152,7 +152,7 @@ + yaw_deg="15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/> @@ -160,7 +160,7 @@ + yaw_deg="-15.0" pos_tol="0.25" ori_tol_deg="6.0" msec="3000" ctx="{ctx}"/> diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/start_gate_mission.xml b/src/subjugator/mission_planner/subjugator_missions/xml/start_gate_mission.xml index e685afe6..933e9a06 100644 --- a/src/subjugator/mission_planner/subjugator_missions/xml/start_gate_mission.xml +++ b/src/subjugator/mission_planner/subjugator_missions/xml/start_gate_mission.xml @@ -58,17 +58,24 @@ + - + + + + + diff --git a/src/subjugator/mission_planner/subjugator_operations/include/hone_bearing.hpp b/src/subjugator/mission_planner/subjugator_operations/include/hone_bearing.hpp index 1a02e2d4..9cef0495 100644 --- a/src/subjugator/mission_planner/subjugator_operations/include/hone_bearing.hpp +++ b/src/subjugator/mission_planner/subjugator_operations/include/hone_bearing.hpp @@ -3,10 +3,13 @@ #include #include +#include #include #include "context.hpp" +#include + 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 findBestDet_(yolo_msgs::msg::DetectionArray const& arr, std::string const& label, + double min_conf); + + static std::optional bearingLinearDeg_(double cx_px, uint32_t width_px, double hfov_deg); + static std::optional bearingAtanDeg_(double cx_px, uint32_t width_px, double hfov_deg); + std::shared_ptr ctx_; - bool published_{ false }; }; diff --git a/src/subjugator/mission_planner/subjugator_operations/include/yaw_p_controller.hpp b/src/subjugator/mission_planner/subjugator_operations/include/yaw_p_controller.hpp new file mode 100644 index 00000000..660778f9 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/include/yaw_p_controller.hpp @@ -0,0 +1,59 @@ +// yaw_p_controller.hpp +#pragma once + +#include + +#include +#include +#include + +#include + +#include "context.hpp" + +#include + +/** + * 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 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 ctx_{}; + + // Stateful bookkeeping across ticks + int stable_count_{ 0 }; + + // Timing (seconds) + double start_t_{ 0.0 }; + double last_det_t_{ 0.0 }; +}; diff --git a/src/subjugator/mission_planner/subjugator_operations/src/hone_bearing.cpp b/src/subjugator/mission_planner/subjugator_operations/src/hone_bearing.cpp index 24bed70a..488bfb77 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/hone_bearing.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/hone_bearing.cpp @@ -14,17 +14,35 @@ #define M_PI 3.14159265358979323846 #endif +/*This operation does not fail immediately if no image data has been received in an attempt to account for delays from + * camera on startup*/ + HoneBearing::HoneBearing(std::string const& name, const BT::NodeConfiguration& cfg) : BT::StatefulActionNode(name, cfg) { } BT::PortsList HoneBearing::providedPorts() { - return { BT::InputPort("label", "shark", "Target label (single)"), - BT::InputPort("offset_deg", 0.0, "Positive=left yaw bias, negative=right"), - BT::InputPort("fov_deg", 110.0, "Horizontal FOV of camera"), - BT::InputPort("min_conf", 0.30, "Minimum confidence"), - BT::InputPort>("ctx") }; + BT::PortsList ports; + + // Inputs + ports.insert(BT::InputPort("label", "shark", "Target label (single)")); + ports.insert(BT::InputPort("min_conf", 0.30, "Minimum confidence")); + ports.insert(BT::InputPort("offset_deg", 0.0, "Desired bearing offset (deg). Right positive.")); + ports.insert(BT::InputPort("fov_deg", 110.0, "Horizontal FOV (deg)")); + ports.insert(BT::InputPort("method", "atan", "Bearing model: 'linear' or 'atan'")); + ports.insert(BT::InputPort>("ctx", "Shared Context")); + + // Outputs + ports.insert(BT::OutputPort("found")); + ports.insert(BT::OutputPort("best_cx_px")); + ports.insert(BT::OutputPort("best_conf")); + + ports.insert(BT::OutputPort("bearing_deg")); + ports.insert(BT::OutputPort("error_deg")); + ports.insert(BT::OutputPort("yaw_cmd_deg")); + + return ports; } BT::NodeStatus HoneBearing::onStart() @@ -34,29 +52,32 @@ BT::NodeStatus HoneBearing::onStart() RCLCPP_ERROR(rclcpp::get_logger("mission_planner"), "HoneBearing: missing ctx"); return BT::NodeStatus::FAILURE; } - published_ = false; + + setOutput("found", false); + setOutput("bearing_deg", 0.0); + setOutput("error_deg", 0.0); + setOutput("yaw_cmd_deg", 0.0); + setOutput("best_cx_px", 0.0); + setOutput("best_conf", 0.0); + return BT::NodeStatus::RUNNING; } BT::NodeStatus HoneBearing::onRunning() { - if (published_) - { - return BT::NodeStatus::SUCCESS; - } - - // Inputs - std::string label; - double offset_deg = 0.0; - double fov = 110.0; + // Default inputs + std::string label = "shark"; + std::string method = "atan"; double min_conf = 0.30; + double offset_deg = 0.0; + double fov_deg = 110.0; (void)getInput("label", label); - (void)getInput("offset_deg", offset_deg); - (void)getInput("fov_deg", fov); + (void)getInput("method", method); (void)getInput("min_conf", min_conf); + (void)getInput("offset_deg", offset_deg); + (void)getInput("fov_deg", fov_deg); - // Get image width uint32_t W = 0; { std::scoped_lock lk(ctx_->img_mx); @@ -66,10 +87,10 @@ BT::NodeStatus HoneBearing::onRunning() if (W == 0) { RCLCPP_WARN(ctx_->logger(), "HoneBearing: image size unknown"); - return BT::NodeStatus::FAILURE; + return BT::NodeStatus::RUNNING; } - // Best detection + // Get latest detections std::optional arr; { std::scoped_lock lk(ctx_->detections_mx); @@ -78,83 +99,61 @@ BT::NodeStatus HoneBearing::onRunning() if (!arr || arr->detections.empty()) { - return BT::NodeStatus::FAILURE; - } - - yolo_msgs::msg::Detection const* best = nullptr; - double best_conf = 0.0; - for (auto const& d : arr->detections) - { - if (d.class_name == label && d.score >= min_conf && d.score > best_conf) - { - best = &d; - best_conf = d.score; - } + RCLCPP_WARN(ctx_->logger(), "HoneBearing: No detections"); + setOutput("found", false); + return BT::NodeStatus::RUNNING; } + // Find best detection + auto best = findBestDet_(*arr, label, min_conf); if (!best) { - return BT::NodeStatus::FAILURE; + setOutput("found", false); + return BT::NodeStatus::RUNNING; } - // Pixel -> bearing (deg). Right positive. - double cx = best->bbox.center.position.x; - double bearing = ((cx - static_cast(W) / 2.0) / (static_cast(W) / 2.0)) * (fov / 2.0); - - // Turn command (deg). Left positive. - double yaw_cmd_deg = offset_deg - bearing; - - // Compose absolute target orientation: q_out = q_cur * q_delta - geometry_msgs::msg::Pose current{}; + // Calculate bearing + std::optional bearing_deg; + if (method == "linear") { - std::scoped_lock lk(ctx_->odom_mx); - if (ctx_->latest_odom) - { - current = ctx_->latest_odom->pose.pose; - } + bearing_deg = bearingLinearDeg_(best->cx_px, W, fov_deg); } - - double yaw_rad = yaw_cmd_deg * M_PI / 180.0; - - geometry_msgs::msg::Quaternion delta_q{}; - delta_q.x = 0.0; - delta_q.y = 0.0; - delta_q.z = std::sin(yaw_rad / 2.0); - delta_q.w = std::cos(yaw_rad / 2.0); - - auto const& c = current.orientation; - geometry_msgs::msg::Pose out = current; - auto& o = out.orientation; - o.x = c.w * delta_q.x + c.x * delta_q.w + c.y * delta_q.z - c.z * delta_q.y; - o.y = c.w * delta_q.y - c.x * delta_q.z + c.y * delta_q.w + c.z * delta_q.x; - o.z = c.w * delta_q.z + c.x * delta_q.y - c.y * delta_q.x + c.z * delta_q.w; - o.w = c.w * delta_q.w - c.x * delta_q.x - c.y * delta_q.y - c.z * delta_q.z; - - // Normalize - double n = std::sqrt(o.x * o.x + o.y * o.y + o.z * o.z + o.w * o.w); - if (n < 1e-12) + else if (method == "atan") { - o = geometry_msgs::msg::Quaternion{}; - o.w = 1.0; + bearing_deg = bearingAtanDeg_(best->cx_px, W, fov_deg); } else { - o.x /= n; - o.y /= n; - o.z /= n; - o.w /= n; + RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 2000, + "HoneBearing: unknown method '%s' (use 'linear' or 'atan')", method.c_str()); + setOutput("found", false); + return BT::NodeStatus::FAILURE; } - // Publish exactly once - ctx_->goal_pub->publish(out); + // In the case of mathematical errors like dividing by zero + if (!bearing_deg) { - std::scoped_lock lk(ctx_->last_goal_mx); - ctx_->last_goal = out; + RCLCPP_WARN(ctx_->logger(), "HoneBearing: Error Calculating Bearing"); + setOutput("found", false); + return BT::NodeStatus::FAILURE; } - published_ = true; - RCLCPP_INFO(ctx_->logger(), "HoneBearing: bearing=%.1f°, offset=%.1f°, cmd=%.1f°", bearing, offset_deg, - yaw_cmd_deg); + // Take into account optional offset + double const error_deg = (*bearing_deg) - offset_deg; + double const yaw_cmd_deg = -error_deg; + + // Outputs + setOutput("found", true); + setOutput("best_cx_px", best->cx_px); + setOutput("best_conf", best->conf); + + setOutput("bearing_deg", *bearing_deg); + setOutput("error_deg", error_deg); + setOutput("yaw_cmd_deg", yaw_cmd_deg); + + RCLCPP_INFO_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 400, + "HoneBearing[%s]: W=%u cx=%.1f conf=%.2f bearing=%.2f off=%.2f err=%.2f yaw_cmd=%.2f", + method.c_str(), W, best->cx_px, best->conf, *bearing_deg, offset_deg, error_deg, yaw_cmd_deg); return BT::NodeStatus::SUCCESS; } @@ -162,3 +161,83 @@ BT::NodeStatus HoneBearing::onRunning() void HoneBearing::onHalted() { } + +double HoneBearing::deg2rad(double d) +{ + return d * M_PI / 180.0; +} + +double HoneBearing::rad2deg(double r) +{ + return r * 180.0 / M_PI; +} + +std::optional HoneBearing::findBestDet_(yolo_msgs::msg::DetectionArray const& arr, + std::string const& label, double min_conf) +{ + BestDet out{}; + bool found = false; + + // Get highest confidence + for (auto const& d : arr.detections) + { + double const conf = d.score; + + if (d.class_name != label) + continue; + if (conf < min_conf) + continue; + if (!found || conf > out.conf) + { + found = true; + out.conf = conf; + out.cx_px = d.bbox.center.position.x; + } + } + + if (!found) + return std::nullopt; + + return out; +} + +std::optional HoneBearing::bearingLinearDeg_(double cx_px, uint32_t width_px, double hfov_deg) +{ + if (width_px == 0) + return std::nullopt; + if (!(hfov_deg > 0.0 && hfov_deg < 179.0)) + return std::nullopt; + + double const W = static_cast(width_px); + double const cx0 = W * 0.5; + + // Normalized offset [-1, +1] + double const norm = (cx_px - cx0) / cx0; + + // Linear map to [-hfov/2, +hfov/2] + return norm * (hfov_deg * 0.5); +} + +std::optional HoneBearing::bearingAtanDeg_(double cx_px, uint32_t width_px, double hfov_deg) +{ + if (width_px == 0) + return std::nullopt; + if (!(hfov_deg > 0.0 && hfov_deg < 179.0)) + return std::nullopt; + + double const W = static_cast(width_px); + double const cx0 = W * 0.5; + + // fx = (W/2) / tan(hfov/2) + double const hfov_rad = deg2rad(hfov_deg); + double const half = 0.5 * hfov_rad; + double const tan_half = std::tan(half); + if (std::abs(tan_half) < 1e-12) + return std::nullopt; + + double const fx = cx0 / tan_half; + + // theta = atan((x - cx)/fx) + double const theta = std::atan((cx_px - cx0) / fx); + return rad2deg(theta); +} diff --git a/src/subjugator/mission_planner/subjugator_operations/src/track_largest_poles.cpp b/src/subjugator/mission_planner/subjugator_operations/src/track_largest_poles.cpp index 89e33377..6a82017f 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/track_largest_poles.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/track_largest_poles.cpp @@ -140,7 +140,8 @@ BT::NodeStatus TrackLargestPoles::onRunning() continue; double w = det.bbox.size.x; double h = det.bbox.size.y; - double area = std::max(0.0, w) * std::max(0.0, h); + + double area = w * h; if (det.class_name == "red-pole" && area > best_red_area_) { diff --git a/src/subjugator/mission_planner/subjugator_operations/src/yaw_p_controller.cpp b/src/subjugator/mission_planner/subjugator_operations/src/yaw_p_controller.cpp new file mode 100644 index 00000000..1f14cd90 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/src/yaw_p_controller.cpp @@ -0,0 +1,217 @@ +// yaw_p_controller.cpp + +#include "yaw_p_controller.hpp" + +#include +#include +#include +#include + +#include + +YawPController::YawPController(std::string const& name, BT::NodeConfiguration const& cfg) + : BT::StatefulActionNode(name, cfg) +{ +} + +BT::PortsList YawPController::providedPorts() +{ + BT::PortsList ports; + + // Inputs + ports.insert(BT::InputPort("label", "shark", "Target label (single)")); + ports.insert(BT::InputPort("min_conf", 0.30, "Minimum confidence")); + ports.insert(BT::InputPort("kp", 0.02, "P gain: yaw_cmd units per pixel error")); + ports.insert(BT::InputPort("tol_px", 8.0, "Centered tolerance in pixels")); + ports.insert(BT::InputPort("hold_ticks", 10, "Consecutive ticks within tolerance to succeed")); + ports.insert(BT::InputPort("max_cmd", 10.0, "Clamp magnitude for yaw_cmd")); + ports.insert(BT::InputPort>("ctx", "Shared Context")); + + // Outputs + ports.insert(BT::OutputPort("centered")); + ports.insert(BT::OutputPort("best_cx_px")); + ports.insert(BT::OutputPort("best_conf")); + ports.insert(BT::OutputPort("yaw_cmd")); + + return ports; +} + +BT::NodeStatus YawPController::onStart() +{ + if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) + { + RCLCPP_ERROR(rclcpp::get_logger("mission_planner"), "YawPController: missing ctx"); + return BT::NodeStatus::FAILURE; + } + + stable_count_ = 0; + + setOutput("centered", false); + setOutput("best_cx_px", 0.0); + setOutput("best_conf", 0.0); + setOutput("yaw_cmd", 0.0); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus YawPController::onRunning() +{ + // Read inputs + std::string label = "shark"; + double min_conf = 0.30; + double kp = 0.02; + double tol_px = 8.0; + int hold_ticks = 10; + double max_cmd = 10.0; + + (void)getInput("label", label); + (void)getInput("min_conf", min_conf); + (void)getInput("kp", kp); + (void)getInput("tol_px", tol_px); + (void)getInput("hold_ticks", hold_ticks); + (void)getInput("max_cmd", max_cmd); + + // Validate variables + if (hold_ticks < 1) + { + RCLCPP_WARN(ctx_->logger(), "YawPController: hold_ticks must be >= 1"); + return BT::NodeStatus::FAILURE; + } + + if (tol_px < 0.0) + { + RCLCPP_WARN(ctx_->logger(), "YawPController: tol_px must be >= 0"); + return BT::NodeStatus::FAILURE; + } + + if (max_cmd < 0.0) + { + RCLCPP_WARN(ctx_->logger(), "YawPController: max_cmd must be >= 0"); + return BT::NodeStatus::FAILURE; + } + + min_conf = std::clamp(min_conf, 0.0, 1.0); + + // Getting img width + uint32_t W = 0; + { + std::scoped_lock lk(ctx_->img_mx); + W = ctx_->img_width; + } + + if (W == 0) + { + setOutput("centered", false); + setOutput("best_cx_px", 0.0); + setOutput("best_conf", 0.0); + setOutput("yaw_cmd", 0.0); + + stable_count_ = 0; + + RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 1500, "YawPController: image size unknown"); + return BT::NodeStatus::RUNNING; + } + + // Read latest detections + std::optional arr; + { + std::scoped_lock lk(ctx_->detections_mx); + arr = ctx_->latest_detections; + } + + if (!arr || arr->detections.empty()) + { + setOutput("centered", false); + setOutput("best_cx_px", 0.0); + setOutput("best_conf", 0.0); + setOutput("yaw_cmd", 0.0); + stable_count_ = 0; + + RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 1500, "YawPController: no detections"); + return BT::NodeStatus::RUNNING; + } + + auto best = findBestDet_(*arr, label, min_conf); + if (!best) + { + setOutput("centered", false); + setOutput("best_cx_px", 0.0); + setOutput("best_conf", 0.0); + setOutput("yaw_cmd", 0.0); + stable_count_ = 0; + + RCLCPP_WARN_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 1500, + "YawPController: no match for label='%s' min_conf=%.2f", label.c_str(), min_conf); + return BT::NodeStatus::RUNNING; + } + + // Computing pixel error relative to center + double const cx0 = 0.5 * static_cast(W); + double const error_px = (best->cx_px - cx0); + + // P control + double yaw_cmd = -kp * error_px; + yaw_cmd = std::clamp(yaw_cmd, -max_cmd, +max_cmd); + + bool const centered = (std::abs(error_px) <= tol_px); + if (centered) + stable_count_++; + else + stable_count_ = 0; + + // Outputs + setOutput("centered", centered); + setOutput("best_cx_px", best->cx_px); + setOutput("best_conf", best->conf); + setOutput("yaw_cmd", yaw_cmd); + + RCLCPP_INFO_THROTTLE(ctx_->logger(), *ctx_->node->get_clock(), 300, + "YawPController: W=%u cx=%.1f conf=%.2f err_px=%.1f kp=%.4f cmd=%.3f centered=%d stable=%d/%d", + W, best->cx_px, best->conf, error_px, kp, yaw_cmd, centered ? 1 : 0, stable_count_, + hold_ticks); + + // Success + if (stable_count_ >= hold_ticks) + { + setOutput("yaw_cmd", 0.0); + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::RUNNING; +} + +void YawPController::onHalted() +{ + stable_count_ = 0; + setOutput("centered", false); + setOutput("yaw_cmd", 0.0); +} + +std::optional YawPController::findBestDet_(yolo_msgs::msg::DetectionArray const& arr, + std::string const& label, double min_conf) const +{ + BestDet out{}; + bool found = false; + + for (auto const& d : arr.detections) + { + double const conf = d.score; + + if (d.class_name != label) + continue; + if (conf < min_conf) + continue; + + if (!found || conf > out.conf) + { + found = true; + out.conf = conf; + out.cx_px = d.bbox.center.position.x; + } + } + + if (!found) + return std::nullopt; + + return out; +} diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro index 4b39f19c..364fe851 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro @@ -3,7 +3,7 @@ + width=960 height=600 fov=1.148 fps=30"> diff --git a/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world b/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world index 38679ad5..cf1634d7 100644 --- a/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world +++ b/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world @@ -80,7 +80,7 @@ package:://subjugator_description/urdf/sub9.urdf - 2 6 -0.3 0 0 1 + 0 0 -0.3 0 0 1