feat(bt): TDD goal extraction BT node development. Frontier detection, component generation working and tested. Visual artifacts produced during testing, manually validated against costmap extracted from simulation.

Author: jburo1

.vscode/settings.json

@@ -6,6 +6,10 @@
     "C_Cpp.clang_format_style": "file",
     "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
     "C_Cpp.default.compileCommands": "${workspaceFolder}/build/omniseer_description/compile_commands.json",
+    "C_Cpp.default.includePath": [
+        "${workspaceFolder}/include", // so omniseer/... works
+        "${workspaceFolder}/**"
+    ],
     "cmake.ignoreCMakeListsMissing": true,
     "files.associations": {
         "cctype": "cpp",
@@ -88,6 +92,7 @@
         "ranges": "cpp",
         "shared_mutex": "cpp",
         "stdfloat": "cpp",
-        "valarray": "cpp"
+        "valarray": "cpp",
+        "filesystem": "cpp"
     }
 }

ros_ws/src/omniseer_description/CMakeLists.txt

@@ -8,7 +8,6 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(xacro REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(ament_cmake REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -20,12 +19,24 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
-add_library(omniseer_bt_nodes SHARED
-  src/select_frontier_from_costmap.cpp
+# --- Core (ROS-agnostic) ---
+add_library(omniseer_frontier_core STATIC
   src/frontier.cpp
   src/grid_io.cpp
 )
 
+target_include_directories(omniseer_frontier_core PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/third_party>
+  $<INSTALL_INTERFACE:include>)
+
+target_compile_features(omniseer_frontier_core PUBLIC cxx_std_17)
+
+# --- ROS BT plugin ---
+add_library(omniseer_bt_nodes SHARED
+  src/select_frontier_from_costmap.cpp
+)
+
 target_include_directories(omniseer_bt_nodes PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/third_party>
@@ -46,40 +57,54 @@ target_link_libraries(omniseer_bt_nodes
     tf2_geometry_msgs::tf2_geometry_msgs
     tf2_ros::tf2_ros)
 
+target_link_libraries(omniseer_bt_nodes
+  PRIVATE omniseer_frontier_core
+)
+
 pluginlib_export_plugin_description_file(behaviortree_cpp plugin_description.xml)
 
+# --- Install ---
 install(DIRECTORY urdf meshes rviz behavior_trees
   DESTINATION share/${PROJECT_NAME}
   OPTIONAL)
 
 install(DIRECTORY include/ DESTINATION include)
 
-install(TARGETS omniseer_bt_nodes
+install(TARGETS omniseer_bt_nodes omniseer_frontier_core
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
 if(BUILD_TESTING)
-  # grid io test
   find_package(ament_cmake_gtest REQUIRED)
+
+  set(OMNISEER_ARTIFACTS_DIR "${CMAKE_CURRENT_BINARY_DIR}/artifacts")
+  set(TEST_INC "${CMAKE_CURRENT_SOURCE_DIR}/test/include")
+
   ament_add_gtest(test_grid_io test/test_grid_io.cpp)
-  target_link_libraries(test_grid_io omniseer_bt_nodes)
-  target_compile_definitions(test_grid_io PRIVATE TEST_DIR="${CMAKE_SOURCE_DIR}")
-
-  # # xacro regeneration test
-  # ament_add_test(check_xacro
-  #   COMMAND bash -c "
-  #     xacro ${CMAKE_SOURCE_DIR}/urdf/xacro/exported_robot.urdf.xacro \
-  #       > ${CMAKE_BINARY_DIR}/rebuilt.urdf &&
-  #     diff -u --ignore-all-space ${CMAKE_SOURCE_DIR}/urdf/exported_robot.urdf \
-  #             ${CMAKE_BINARY_DIR}/rebuilt.urdf
-  #   "
-  #   WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-
-  # find_package(ament_lint_auto REQUIRED)
-  # set(ament_cmake_copyright_FOUND TRUE)
-  # set(ament_cmake_cpplint_FOUND TRUE)
-  # ament_lint_auto_find_test_dependencies()
+  target_link_libraries(test_grid_io omniseer_frontier_core)
+  target_include_directories(test_grid_io PRIVATE ${TEST_INC})
+  target_compile_definitions(test_grid_io PRIVATE 
+    TEST_DIR="${CMAKE_SOURCE_DIR}"
+  )
+
+  ament_add_gtest(test_frontier_mask test/test_frontier_mask.cpp)
+  target_link_libraries(test_frontier_mask omniseer_frontier_core)
+  target_include_directories(test_frontier_mask PRIVATE ${TEST_INC})
+  target_compile_definitions(test_frontier_mask PRIVATE 
+    TEST_DIR="${CMAKE_SOURCE_DIR}"
+    OMNISEER_ARTIFACTS_DIR="${OMNISEER_ARTIFACTS_DIR}"
+  )
+
+  ament_add_gtest(test_components test/test_components.cpp)
+  target_link_libraries(test_components omniseer_frontier_core)
+  target_include_directories(test_components PRIVATE ${TEST_INC})
+  target_compile_definitions(test_components PRIVATE 
+    TEST_DIR="${CMAKE_SOURCE_DIR}"
+    OMNISEER_ARTIFACTS_DIR="${OMNISEER_ARTIFACTS_DIR}"
+  )
+
+  # more here
 endif()
 
 ament_package()

ros_ws/src/omniseer_description/include/omniseer/frontier.hpp

@@ -1,3 +1,135 @@
+#pragma once
+
 #include <cstdint>
 #include <functional>
+#include <span>
+#include <string>
+#include <utility>
 #include <vector>
+
+#include "omniseer/grid_io.hpp"
+
+/*
+ROS-agnostic frontier detection and goal selection API test code.
+
+Purpose:
+- Given a costmap (GridU8 from grid_io.hpp), identify frontier cells,
+  group them into connected components, propose feasible goal poses, and rank those
+  goals for exploration based on information gain and distance from current position. Return maximal
+  value goal, or -1 if none exist.
+
+  Types:
+- Pose2D: {x, y} in map frame
+- Conn: 4- or 8-neighborhood for component labeling.
+- Params: thresholds, connectivity, size filters, robot radius/clearance, range caps,
+          and scoring weights.
+*/
+
+namespace omniseer
+{
+
+  struct Pose2D
+  {
+    double x{0.0};
+    double y{0.0};
+  };
+
+  enum class Conn : uint8_t
+  {
+    Four  = 4,
+    Eight = 8
+  };
+
+  struct Params
+  {
+    // --- Costmap thresholds ---
+    uint8_t free_cost_max  = 150; // 0 ..
+    uint8_t unknown_cost   = 255; // unkown
+    uint8_t lethal_cost    = 254; // lethal obstacle
+    uint8_t inscribed_cost = 253; // inscribed obstacle
+
+    // Frontier = cell with at least this many UNKNOWN neighbors.
+    Conn connectivity = Conn::Eight;
+
+    int min_component_size    = 10; // cells; discard smaller components
+    int min_unknown_neighbors = 1;  // 1..8 depending on connectivity
+
+    // --- Goal generation / validation ---
+    double robot_radius_m      = 0.20;
+    double goal_clearance_m    = 0.10;
+    double max_goal_distance_m = 20.0;
+
+    double w_information   = 1.0;
+    double w_distance_cost = 1.0;
+  };
+
+  struct Callbacks
+  {
+    // Return true if goal is achievable from current position
+    std::function<bool(const Pose2D& goal, double robot_radius_m, double clearance_m)>
+        is_pose_navigable;
+
+    // Planning/path cost from robot to goal
+    std::function<double(const Pose2D& robot, const Pose2D& goal)> plan_cost;
+
+    // Information gain estimate from robot to goal
+    std::function<double(const Pose2D& goal)> information_gain;
+
+    // Logging hook
+    std::function<void(const std::string& msg)> log;
+  };
+
+  struct FrontierComponent
+  {
+    int              id{-1};               // 0..K-1
+    int              size_cells{0};        // number of frontier cells
+    std::vector<int> rim_indices;          // representative frontier cell indices (y*w + x)
+    double           cx_m{0.0}, cy_m{0.0}; // centroid
+  };
+
+  struct FrontierGoal
+  {
+    Pose2D pose;
+    double score{0.0};
+    int    component_id{-1};
+  };
+
+  struct Artifacts
+  {
+    int                       width{0}, height{0}; // dimensions for per-cell arrays
+    std::vector<std::uint8_t> frontier_mask;       // 0/1
+    std::vector<std::int32_t> component_labels;    // -1 or component id
+    std::vector<float>        distance_field_m;    // >=0 or +inf
+    std::vector<float>        goal_heatmap;        // arbitrary finite or NaN
+
+    std::vector<FrontierGoal> goals_pre_rank;
+    std::vector<FrontierGoal> goals_ranked;
+  };
+
+  // --- Pipeline API ---
+
+  // Compute frontier from costmap, i.e. the border between freeish and unknown
+  //   on return: out_mask size = g.width * g.height 0 = not frontier, 1 = frontier
+  void compute_frontier_mask(const GridU8& g, const Params& p, std::uint8_t* out_mask,
+                             std::size_t n);
+
+  // Compute connected components on the frontier using BFS, store metadata
+  //   on entry:  labels[i] = 0 for non-frontier, 1 for frontier (from compute_frontier_mask)
+  //   on return: labels[i] = -1 for non-frontier, or component id 0..K-1 for frontier
+  std::vector<FrontierComponent> label_components(const GridU8& g, const Params& p,
+                                                  std::uint8_t* frontier_mask, std::size_t n);
+
+  // Propose feasible goals per component using callbacks for validity/cost
+  std::vector<FrontierGoal> select_component_goals(const GridU8& g, const Params& p,
+                                                   const Pose2D& robot, const Callbacks& cb,
+                                                   const std::vector<FrontierComponent>& comps);
+
+  // Score and sort goals in-place (descending by score)
+  void rank_goals(const Params& p, const Pose2D& robot, std::vector<FrontierGoal>& goals);
+
+  // Full pipeline with optional debug artifacts sink
+  std::vector<FrontierGoal> find_frontier_goals(const GridU8& g, const Params& p,
+                                                const Pose2D& robot, const Callbacks& cb,
+                                                Artifacts* artifacts);
+
+} // namespace omniseer

ros_ws/src/omniseer_description/include/omniseer/grid_io.hpp

@@ -2,13 +2,25 @@
 #include <cstdint>
 #include <string>
 #include <vector>
+/*
+Representation of PGM costmap (see /examples)
 
-struct GridU8
+data:
+- white (255) : unknown
+- 254/253     : obstacles
+- gray        : inflation gradient
+- black (0)   : free space
+*/
+namespace omniseer
 {
-  uint32_t             width{0}, height{0};
-  float                resolution{0.f};
-  float                origin_x{0.f}, origin_y{0.f};
-  std::vector<uint8_t> data;
-};
 
-GridU8 load_pgm_with_meta(const std::string& pgm_path, const std::string& meta_json_path);
+  struct GridU8
+  {
+    uint32_t             width{0}, height{0};
+    float                resolution{0.f};
+    float                origin_x{0.f}, origin_y{0.f};
+    std::vector<uint8_t> data;
+  };
+
+  GridU8 load_pgm_with_meta(const std::string& pgm_path, const std::string& meta_json_path);
+} // namespace omniseer

ros_ws/src/omniseer_description/scripts/view_frontier_components.sh

@@ -0,0 +1,7 @@
+# Visual inspection of /frontier.cpp's compute_frontier_mask output
+ montage \
+  -label 'Rims'     components_rims.ppm \
+  -label 'Frontier' frontier_mask.pgm \
+  -label 'Rims Overlay'  rims_overlay.ppm \
+  -tile 2x2 -geometry +6+6 -background black -fill white -pointsize 18 \
+  frontier_montage.png && xdg-open frontier_montage.png || echo "See frontier_montage.png"

ros_ws/src/omniseer_description/scripts/view_frontier_mask.sh

@@ -0,0 +1,8 @@
+# Visual inspection of /frontier.cpp's compute_frontier_mask output
+ montage \
+  -label 'Free'     free_mask.pgm \
+  -label 'Unknown'  unknown_mask.pgm \
+  -label 'Frontier' frontier_mask.pgm \
+  -label 'Overlay'  frontier_overlay.ppm \
+  -tile 2x2 -geometry +6+6 -background black -fill white -pointsize 18 \
+  frontier_montage.png && xdg-open frontier_montage.png || echo "See frontier_montage.png"