Skip to content

Commit d99261d

Browse files
mertgulerxmertgulerx
committed
Add Debug Observer RViz plugin
1 parent da3c483 commit d99261d

9 files changed

Lines changed: 2165 additions & 16 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,28 @@ ament_target_dependencies(frontier_exploration_ctl
9797
rclcpp
9898
)
9999

100+
# Passive RViz debug observer. It links against the core library to reuse the
101+
# same frontier extraction and scoring code, but it is installed as a separate
102+
# executable so visualization never becomes part of the navigation control path.
103+
add_executable(frontier_debug_observer
104+
src/debug/debug_observer_node.cpp
105+
src/debug/debug_analyzer.cpp
106+
src/debug/debug_markers.cpp
107+
)
108+
target_compile_features(frontier_debug_observer PUBLIC cxx_std_17)
109+
target_link_libraries(frontier_debug_observer
110+
${PROJECT_NAME}_core
111+
)
112+
ament_target_dependencies(frontier_debug_observer
113+
geometry_msgs
114+
nav_msgs
115+
rclcpp
116+
std_msgs
117+
tf2
118+
tf2_ros
119+
visualization_msgs
120+
)
121+
100122
if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
101123
include(CheckIPOSupported)
102124
check_ipo_supported(RESULT ipo_supported OUTPUT ipo_error)
@@ -105,6 +127,7 @@ if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebI
105127
set_property(TARGET ${PROJECT_NAME}_node PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
106128
set_property(TARGET frontier_explorer PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
107129
set_property(TARGET frontier_exploration_ctl PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
130+
set_property(TARGET frontier_debug_observer PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
108131
endif()
109132
endif()
110133

@@ -122,6 +145,7 @@ install(
122145
TARGETS
123146
frontier_explorer
124147
frontier_exploration_ctl
148+
frontier_debug_observer
125149
RUNTIME DESTINATION lib/${PROJECT_NAME}
126150
)
127151

‎README.md‎

Lines changed: 291 additions & 16 deletions
Large diffs are not rendered by default.

‎include/frontier_exploration_ros2/debug/debug_analyzer.hpp‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
/*
2+
Copyright 2026 Mert Guler
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
http://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
17+
#pragma once
18+
19+
#include <cstddef>
20+
#include <optional>
21+
#include <string>
22+
#include <vector>
23+
24+
#include <geometry_msgs/msg/pose.hpp>
25+
#include <nav_msgs/msg/occupancy_grid.hpp>
26+
27+
#include "frontier_exploration_ros2/frontier_search.hpp"
28+
#include "frontier_exploration_ros2/frontier_types.hpp"
29+
30+
namespace frontier_exploration_ros2::debug
31+
{
32+
33+
// Configuration mirrored by the passive debug observer. The analyzer uses these
34+
// values to reproduce frontier extraction and scoring decisions without owning
35+
// navigation state, sending goals, or changing the running explorer behavior.
36+
struct DebugAnalyzerConfig
37+
{
38+
// Frontier extraction and map optimization settings must match the explorer
39+
// node so raw and optimized overlays explain the same candidate set.
40+
FrontierStrategy strategy{FrontierStrategy::NEAREST};
41+
bool frontier_map_optimization_enabled{true};
42+
double sigma_s{2.0};
43+
double sigma_r{30.0};
44+
int dilation_kernel_radius_cells{1};
45+
int occ_threshold{OCC_THRESHOLD};
46+
int min_frontier_size_cells{MIN_FRONTIER_SIZE};
47+
double frontier_candidate_min_goal_distance_m{0.0};
48+
double frontier_selection_min_distance{0.5};
49+
double frontier_visit_tolerance{0.30};
50+
bool escape_enabled{true};
51+
52+
// MRTSP and DP parameters are only used for analysis. They build the same
53+
// cost matrix, pruning pool, and bounded-horizon sequence shown in RViz.
54+
std::string mrtsp_solver{"dp"};
55+
std::size_t dp_solver_candidate_limit{15};
56+
std::size_t dp_planning_horizon{10};
57+
double sensor_effective_range_m{1.5};
58+
double weight_distance_wd{1.0};
59+
double weight_gain_ws{1.0};
60+
double max_linear_speed_vmax{0.5};
61+
double max_angular_speed_wmax{1.0};
62+
bool analyze_nearest_scores{true};
63+
bool analyze_mrtsp_scores{true};
64+
bool analyze_dp_pruning{true};
65+
};
66+
67+
// Per-candidate explanation data used by the RViz marker layer. The candidate
68+
// itself stays intact while this struct adds nearest, MRTSP, and DP annotations.
69+
struct FrontierDebugCandidate
70+
{
71+
std::size_t id{};
72+
FrontierCandidate candidate;
73+
74+
// Nearest selection uses the centroid as the ordering reference and the
75+
// dispatch point for visit-tolerance checks.
76+
std::pair<double, double> reference_point{0.0, 0.0};
77+
std::pair<double, double> dispatch_point{0.0, 0.0};
78+
double nearest_reference_distance{0.0};
79+
double nearest_goal_distance{0.0};
80+
bool nearest_visit_tolerance_skip{false};
81+
bool nearest_preferred_pool{false};
82+
bool nearest_selected{false};
83+
std::string nearest_mode;
84+
85+
// MRTSP score breakdown mirrors the start-row cost used by the matrix. Keeping
86+
// the parts separate makes weight, gain, path, and time effects visible.
87+
double mrtsp_gain{0.0};
88+
double mrtsp_initial_path_cost{0.0};
89+
double mrtsp_motion_time_cost{0.0};
90+
double mrtsp_start_cost{0.0};
91+
std::optional<std::size_t> mrtsp_greedy_rank;
92+
93+
// DP annotations preserve the distinction between pruning rank and route rank:
94+
// a candidate can be in the DP pool without appearing in the selected sequence.
95+
bool dp_pruned{false};
96+
std::optional<std::size_t> dp_prune_rank;
97+
std::optional<std::size_t> dp_order_rank;
98+
bool active_order_selected{false};
99+
};
100+
101+
// Full analysis result for one observer tick. Marker publishers consume this
102+
// snapshot directly, keeping visualization formatting separate from scoring.
103+
struct FrontierDebugSnapshot
104+
{
105+
std::vector<FrontierCandidate> raw_frontiers;
106+
std::vector<FrontierCandidate> optimized_frontiers;
107+
std::vector<FrontierDebugCandidate> candidates;
108+
std::vector<std::size_t> nearest_order;
109+
std::vector<std::size_t> mrtsp_greedy_order;
110+
std::vector<std::size_t> dp_pruned_indices;
111+
std::vector<std::size_t> dp_order;
112+
std::vector<std::size_t> active_order;
113+
nav_msgs::msg::OccupancyGrid decision_map_msg;
114+
std::string active_selection_mode;
115+
};
116+
117+
// Builds a read-only debug snapshot from map, costmap, pose, and parameters.
118+
// The function intentionally returns data only; it does not publish, dispatch,
119+
// cancel, suppress, or mutate exploration state.
120+
FrontierDebugSnapshot analyze_frontier_debug_snapshot(
121+
const geometry_msgs::msg::Pose & current_pose,
122+
const OccupancyGrid2d & map,
123+
const OccupancyGrid2d & costmap,
124+
const std::optional<OccupancyGrid2d> & local_costmap,
125+
const DebugAnalyzerConfig & config);
126+
127+
} // namespace frontier_exploration_ros2::debug

‎include/frontier_exploration_ros2/debug/debug_markers.hpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
/*
2+
Copyright 2026 Mert Guler
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
http://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
17+
#pragma once
18+
19+
#include <cstddef>
20+
#include <string>
21+
22+
#include <geometry_msgs/msg/pose.hpp>
23+
#include <visualization_msgs/msg/marker_array.hpp>
24+
25+
#include "frontier_exploration_ros2/debug/debug_analyzer.hpp"
26+
27+
namespace frontier_exploration_ros2::debug
28+
{
29+
30+
// Shared RViz marker styling. The observer keeps these values separate from the
31+
// analyzer so score computation is independent from how overlays are displayed.
32+
struct DebugMarkerConfig
33+
{
34+
std::string frame_id{"map"};
35+
double point_scale{0.15};
36+
double selected_scale{0.30};
37+
double line_width{0.04};
38+
double text_scale{0.22};
39+
double z_offset{0.05};
40+
bool labels_enabled{true};
41+
std::size_t label_top_n{30};
42+
std::size_t edge_top_n{15};
43+
};
44+
45+
// Frontier set before decision-map optimization. This overlay is useful for
46+
// checking what the map contributes before filtering and smoothing change it.
47+
visualization_msgs::msg::MarkerArray make_raw_frontier_markers(
48+
const FrontierDebugSnapshot & snapshot,
49+
const DebugMarkerConfig & config);
50+
51+
// Frontier set after decision-map optimization, plus the active first target.
52+
visualization_msgs::msg::MarkerArray make_optimized_frontier_markers(
53+
const FrontierDebugSnapshot & snapshot,
54+
const DebugMarkerConfig & config);
55+
56+
// Nearest strategy explanation: candidate pool, rank, mode, and distances.
57+
visualization_msgs::msg::MarkerArray make_nearest_score_markers(
58+
const FrontierDebugSnapshot & snapshot,
59+
const DebugMarkerConfig & config);
60+
61+
// MRTSP start-row score explanation: rank, gain, path cost, and time cost.
62+
visualization_msgs::msg::MarkerArray make_mrtsp_score_markers(
63+
const FrontierDebugSnapshot & snapshot,
64+
const DebugMarkerConfig & config);
65+
66+
// MRTSP or DP route visualization. The line shows the analyzed sequence, while
67+
// labels identify the order in which candidates appear in that sequence.
68+
visualization_msgs::msg::MarkerArray make_mrtsp_order_markers(
69+
const FrontierDebugSnapshot & snapshot,
70+
const geometry_msgs::msg::Pose & current_pose,
71+
const DebugMarkerConfig & config);
72+
73+
// DP pruning visualization. Orange points are passed into bounded-horizon DP;
74+
// grey points are outside the pruned candidate pool.
75+
visualization_msgs::msg::MarkerArray make_dp_pruning_markers(
76+
const FrontierDebugSnapshot & snapshot,
77+
const DebugMarkerConfig & config);
78+
79+
} // namespace frontier_exploration_ros2::debug

‎launch/frontier_debug.launch.py‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
from pathlib import Path
2+
from typing import Any
3+
4+
import yaml
5+
6+
from ament_index_python.packages import get_package_share_directory
7+
from launch import LaunchDescription
8+
from launch.actions import DeclareLaunchArgument
9+
from launch.actions import OpaqueFunction
10+
from launch.substitutions import LaunchConfiguration
11+
from launch_ros.actions import Node
12+
13+
14+
def _load_shared_params(params_path: str) -> dict[str, Any]:
15+
path = Path(params_path).expanduser()
16+
if not path.exists():
17+
return {}
18+
19+
with path.open("r", encoding="utf-8") as stream:
20+
data = yaml.safe_load(stream) or {}
21+
22+
# Parameters are merged in the same order ROS users normally expect:
23+
# wildcard defaults, explorer-specific values, then debug-specific overrides.
24+
# This lets the observer mirror the explorer while still allowing RViz-only
25+
# debug settings to live in the same YAML file.
26+
shared = {}
27+
wildcard_params = data.get("/**", {}).get("ros__parameters", {})
28+
if isinstance(wildcard_params, dict):
29+
shared.update(wildcard_params)
30+
31+
frontier_params = data.get("frontier_explorer", {}).get("ros__parameters", {})
32+
if isinstance(frontier_params, dict):
33+
shared.update(frontier_params)
34+
35+
debug_params = data.get("frontier_debug_observer", {}).get("ros__parameters", {})
36+
if isinstance(debug_params, dict):
37+
shared.update(debug_params)
38+
39+
return shared
40+
41+
42+
def _create_debug_actions(context):
43+
namespace = LaunchConfiguration("namespace")
44+
params_file = LaunchConfiguration("params_file").perform(context)
45+
use_sim_time = LaunchConfiguration("use_sim_time")
46+
log_level = LaunchConfiguration("log_level")
47+
48+
debug_params = _load_shared_params(params_file)
49+
# Launch arguments override the YAML for fast RViz tuning. These parameters
50+
# affect only the observer's display/update behavior, not exploration logic.
51+
debug_params.update(
52+
{
53+
"use_sim_time": use_sim_time,
54+
"debug_update_rate_hz": LaunchConfiguration("debug_update_rate_hz"),
55+
"debug_labels_enabled": LaunchConfiguration("debug_labels_enabled"),
56+
"debug_label_top_n": LaunchConfiguration("debug_label_top_n"),
57+
"debug_edge_top_n": LaunchConfiguration("debug_edge_top_n"),
58+
}
59+
)
60+
61+
return [
62+
Node(
63+
package="frontier_exploration_ros2",
64+
executable="frontier_debug_observer",
65+
name="frontier_debug_observer",
66+
namespace=namespace,
67+
output="screen",
68+
arguments=["--ros-args", "--log-level", log_level],
69+
parameters=[debug_params],
70+
)
71+
]
72+
73+
74+
def generate_launch_description():
75+
default_params = Path(
76+
get_package_share_directory("frontier_exploration_ros2")
77+
) / "config" / "params.yaml"
78+
79+
# The debug observer runs as a standalone node. It can be launched next to an
80+
# explorer instance or by itself against recorded map/costmap/TF data.
81+
return LaunchDescription(
82+
[
83+
DeclareLaunchArgument(
84+
"namespace",
85+
default_value="",
86+
description="Optional namespace for the frontier debug observer.",
87+
),
88+
DeclareLaunchArgument(
89+
"params_file",
90+
default_value=str(default_params),
91+
description="Parameter file used to mirror frontier_explorer settings.",
92+
),
93+
DeclareLaunchArgument(
94+
"use_sim_time",
95+
default_value="false",
96+
description="Use simulation time.",
97+
),
98+
DeclareLaunchArgument(
99+
"log_level",
100+
default_value="info",
101+
description="Log level (debug, info, warn, error, fatal).",
102+
),
103+
DeclareLaunchArgument(
104+
"debug_update_rate_hz",
105+
default_value="1.0",
106+
description="Debug overlay update frequency.",
107+
),
108+
DeclareLaunchArgument(
109+
"debug_labels_enabled",
110+
default_value="true",
111+
description="Publish text labels for top-ranked debug candidates.",
112+
),
113+
DeclareLaunchArgument(
114+
"debug_label_top_n",
115+
default_value="30",
116+
description="Maximum number of candidate labels per overlay.",
117+
),
118+
DeclareLaunchArgument(
119+
"debug_edge_top_n",
120+
default_value="15",
121+
description="Maximum number of MRTSP start edges drawn in RViz.",
122+
),
123+
OpaqueFunction(function=_create_debug_actions),
124+
]
125+
)

‎package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
<exec_depend>ament_index_python</exec_depend>
2424
<exec_depend>launch</exec_depend>
2525
<exec_depend>launch_ros</exec_depend>
26+
<exec_depend>python3-yaml</exec_depend>
2627
<exec_depend>rosidl_default_runtime</exec_depend>
2728

2829
<test_depend>ament_cmake_gtest</test_depend>

0 commit comments

Comments
 (0)