Skip to content

Commit 20bac29

Browse files
JuliajJuliaj
committed
Initial commit for rai_semap
1 parent 7626288 commit 20bac29

29 files changed

+4750
-0
lines changed

‎examples/check-detection-pipeline.py‎

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
# Copyright (C) 2025 Julia Jia
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
"""Diagnostic script to check if the detection pipeline is working."""
16+
17+
import rclpy
18+
from rclpy.node import Node
19+
from sensor_msgs.msg import Image
20+
21+
from rai_interfaces.msg import RAIDetectionArray
22+
23+
24+
class DetectionPipelineChecker(Node):
25+
"""Check if detection pipeline components are working."""
26+
27+
def __init__(self):
28+
super().__init__("detection_pipeline_checker")
29+
self.camera_received = False
30+
self.detections_received = False
31+
self.detection_count = 0
32+
33+
def check_camera_topic(
34+
self, topic: str = "/camera/image_raw", timeout: float = 5.0
35+
):
36+
"""Check if camera topic is publishing."""
37+
self.get_logger().info(f"Checking camera topic: {topic}")
38+
_ = self.create_subscription(
39+
Image, topic, lambda msg: setattr(self, "camera_received", True), 10
40+
)
41+
42+
import time
43+
44+
start_time = time.time()
45+
while time.time() - start_time < timeout:
46+
rclpy.spin_once(self, timeout_sec=0.5)
47+
if self.camera_received:
48+
self.get_logger().info(f"✓ Camera topic {topic} is publishing")
49+
return True
50+
51+
self.get_logger().warn(
52+
f"✗ Camera topic {topic} not publishing (timeout: {timeout}s)"
53+
)
54+
return False
55+
56+
def check_detection_topic(
57+
self, topic: str = "/detection_array", timeout: float = 10.0
58+
):
59+
"""Check if detection topic is publishing."""
60+
self.get_logger().info(f"Checking detection topic: {topic}")
61+
62+
def detection_callback(msg: RAIDetectionArray):
63+
self.detections_received = True
64+
self.detection_count += len(msg.detections)
65+
self.get_logger().info(
66+
f"Received detection array with {len(msg.detections)} detections: "
67+
f"{msg.detection_classes}"
68+
)
69+
70+
_ = self.create_subscription(RAIDetectionArray, topic, detection_callback, 10)
71+
72+
import time
73+
74+
start_time = time.time()
75+
while time.time() - start_time < timeout:
76+
rclpy.spin_once(self, timeout_sec=0.5)
77+
if self.detections_received:
78+
self.get_logger().info(
79+
f"✓ Detection topic {topic} is publishing ({self.detection_count} total detections)"
80+
)
81+
return True
82+
83+
self.get_logger().warn(
84+
f"✗ Detection topic {topic} not publishing (timeout: {timeout}s)"
85+
)
86+
return False
87+
88+
def check_dino_service(
89+
self, service_name: str = "/grounding_dino/grounding_dino_classify"
90+
):
91+
"""Check if DINO service is available."""
92+
from rai_interfaces.srv import RAIGroundingDino
93+
94+
client = self.create_client(RAIGroundingDino, service_name)
95+
96+
self.get_logger().info(f"Checking DINO service: {service_name}")
97+
if client.wait_for_service(timeout_sec=2.0):
98+
self.get_logger().info(f"✓ DINO service {service_name} is available")
99+
return True
100+
else:
101+
self.get_logger().warn(f"✗ DINO service {service_name} not available")
102+
return False
103+
104+
105+
def main():
106+
"""Run diagnostic checks."""
107+
rclpy.init()
108+
checker = DetectionPipelineChecker()
109+
110+
print("\n" + "=" * 60)
111+
print("Detection Pipeline Diagnostic")
112+
print("=" * 60 + "\n")
113+
114+
# Check DINO service
115+
dino_ok = checker.check_dino_service()
116+
print()
117+
118+
# Check camera topic
119+
camera_ok = checker.check_camera_topic()
120+
print()
121+
122+
# Check detection topic (wait longer since it depends on camera)
123+
if camera_ok:
124+
detection_ok = checker.check_detection_topic(timeout=15.0)
125+
else:
126+
print("Skipping detection topic check (camera not available)")
127+
detection_ok = False
128+
print()
129+
130+
# Summary
131+
print("=" * 60)
132+
print("Summary:")
133+
print(f" DINO Service: {'✓' if dino_ok else '✗'}")
134+
print(f" Camera Topic: {'✓' if camera_ok else '✗'}")
135+
print(f" Detection Topic: {'✓' if detection_ok else '✗'}")
136+
print("=" * 60)
137+
138+
if not dino_ok:
139+
print(
140+
"\n⚠️ DINO service not available. Make sure perception agents are running:"
141+
)
142+
print(" python -m rai_perception.scripts.run_perception_agents")
143+
144+
if not camera_ok:
145+
print("\n⚠️ Camera topic not publishing. Check:")
146+
print(" ros2 topic list | grep camera")
147+
print(" ros2 topic echo /camera/image_raw --once")
148+
149+
if not detection_ok and camera_ok:
150+
print("\n⚠️ Detection topic not publishing. Check:")
151+
print(" - Is detection_publisher node running?")
152+
print(" - Check detection_publisher logs for errors")
153+
print(" - Verify camera topic name matches detection_publisher config")
154+
155+
checker.destroy_node()
156+
rclpy.shutdown()
157+
158+
159+
if __name__ == "__main__":
160+
main()

‎examples/rai-semap.launch.py‎

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
# Copyright (C) 2025 Julia Jia
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from launch import LaunchDescription
16+
from launch.actions import DeclareLaunchArgument, ExecuteProcess
17+
from launch.substitutions import LaunchConfiguration
18+
19+
20+
def generate_launch_description():
21+
# Declare launch arguments
22+
database_path_arg = DeclareLaunchArgument(
23+
"database_path",
24+
default_value="semantic_map.db",
25+
description="Path to SQLite database file for semantic map",
26+
)
27+
28+
location_id_arg = DeclareLaunchArgument(
29+
"location_id",
30+
default_value="default_location",
31+
description="Identifier for the physical location",
32+
)
33+
34+
camera_topic_arg = DeclareLaunchArgument(
35+
"camera_topic",
36+
default_value="/camera/camera/color/image_raw",
37+
description="Camera image topic",
38+
)
39+
40+
depth_topic_arg = DeclareLaunchArgument(
41+
"depth_topic",
42+
default_value="/camera/camera/depth/image_rect_raw",
43+
description="Depth image topic",
44+
)
45+
46+
camera_info_topic_arg = DeclareLaunchArgument(
47+
"camera_info_topic",
48+
default_value="/camera/camera/color/camera_info",
49+
description="Camera info topic",
50+
)
51+
52+
# Launch detection publisher (bridges DINO service to /detection_array topic)
53+
camera_topic_param = LaunchConfiguration("camera_topic")
54+
depth_topic_param = LaunchConfiguration("depth_topic")
55+
camera_info_topic_param = LaunchConfiguration("camera_info_topic")
56+
57+
launch_detection_publisher = ExecuteProcess(
58+
cmd=[
59+
"python",
60+
"-m",
61+
"rai_semap.ros2.detection_publisher",
62+
"--ros-args",
63+
"-p",
64+
["camera_topic:=", camera_topic_param],
65+
"-p",
66+
["depth_topic:=", depth_topic_param],
67+
"-p",
68+
["camera_info_topic:=", camera_info_topic_param],
69+
"-p",
70+
"detection_topic:=/detection_array",
71+
"-p",
72+
"dino_service:=/grounding_dino_classify",
73+
"-p",
74+
"detection_interval:=2.0",
75+
"-p",
76+
"box_threshold:=0.4",
77+
"-p",
78+
"text_threshold:=0.3",
79+
],
80+
output="screen",
81+
)
82+
83+
# Launch semantic map node
84+
db_path_param = LaunchConfiguration("database_path")
85+
loc_id_param = LaunchConfiguration("location_id")
86+
87+
launch_semap = ExecuteProcess(
88+
cmd=[
89+
"python",
90+
"-m",
91+
"rai_semap.ros2.node",
92+
"--ros-args",
93+
"-p",
94+
["database_path:=", db_path_param],
95+
"-p",
96+
["location_id:=", loc_id_param],
97+
"-p",
98+
"confidence_threshold:=0.5",
99+
"-p",
100+
"class_confidence_thresholds:=person:0.7,window:0.6,door:0.5",
101+
"-p",
102+
"class_merge_thresholds:=couch:2.5,table:1.5,shelf:1.5,chair:0.8",
103+
"-p",
104+
"min_bbox_area:=500.0",
105+
"-p",
106+
"use_pointcloud_dedup:=true",
107+
"-p",
108+
["depth_topic:=", depth_topic_param],
109+
"-p",
110+
["camera_info_topic:=", camera_info_topic_param],
111+
"-p",
112+
"detection_topic:=/detection_array",
113+
"-p",
114+
"map_topic:=/map",
115+
"-p",
116+
"map_frame_id:=map",
117+
"-p",
118+
"map_resolution:=0.05",
119+
],
120+
output="screen",
121+
)
122+
123+
return LaunchDescription(
124+
[
125+
database_path_arg,
126+
location_id_arg,
127+
camera_topic_arg,
128+
depth_topic_arg,
129+
camera_info_topic_arg,
130+
launch_detection_publisher,
131+
launch_semap,
132+
]
133+
)

0 commit comments

Comments
 (0)