wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Unify navigation on NavigateToPose and remove legacy proxies

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 17:40:25 +08:00
parent 964d1fde67
commit 24e0b922bc
40 changed files with 461 additions and 2592 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/helpers/live_mock_smoke.py
+2 -5
View File
@@ -33,9 +33,6 @@ def goal_for(name):
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.orientation.w = 1.0
goal.position_tolerance = goal.yaw_tolerance = 0.1
elif name == "navigate_semantic":
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
goal.position_tolerance = goal.orientation_tolerance = 0.1
elif name == "execute_manipulation":
goal.skill, goal.instruction = "pick", "pick the smoke target"
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
@@ -111,8 +108,8 @@ def main():
if not cancel.goals_canceling:
raise RuntimeError("cancel was not acknowledged")
canceled = await_future(handle.get_result_async())
if (canceled.result.result.status != canceled.result.result.CANCELED or
canceled.result.result.stop_state != canceled.result.result.UNKNOWN):
if (canceled.result.status != canceled.result.CANCELED or
canceled.result.stop_state != canceled.result.STOP_UNKNOWN):
raise RuntimeError("cancel terminal did not preserve unknown stop")
state_client = client_node.create_client(GetRobotState, "get_robot_state")
tests/helpers/native_coordinator_regression.py
+2 -2
View File
@@ -38,7 +38,7 @@ def run(root, binary, output):
scenarios = {
'plan_task':[{'kind':'normal', 'plan':plan}],
'verify_state':[{'kind':'passed'}],
'navigate_semantic':[{'kind':'normal','final_pose':site['locations'][name]}
'navigate':[{'kind':'normal','final_pose':site['locations'][name]}
for name in ['shelf_A_stop','tote_A_stop']*2],
}
actions = [name for name in ACTION_TYPES if name != 'execute_task']
@@ -78,7 +78,7 @@ def run(root, binary, output):
def counts():
with server.lock:
return {name:server.counts[name] for name in ('plan_task','navigate_semantic','execute_manipulation','execute_posture')}
return {name:server.counts[name] for name in ('plan_task','navigate','execute_manipulation','execute_posture')}
with tempfile.TemporaryDirectory(prefix='native-coordinator-regression-') as state:
state = Path(state)
tests/helpers/native_navigation_contract.py
+56 -51
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Generated Navigate messages + real DDS + production C++ RosDriver regression.
"""Generated NavigateToPose messages + real DDS + production C++ RosDriver regression.
In a sourced ROS Humble overlay:
python3 tests/helpers/native_navigation_contract.py --build-native --output /tmp/nav-contract
@@ -30,13 +30,14 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(bt_skill_interfaces REQUIRED)
find_package(navigation_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nlohmann_json REQUIRED)
add_executable(native_navigation_probe %s %s %s)
target_include_directories(native_navigation_probe PRIVATE %s %s)
target_link_libraries(native_navigation_probe nlohmann_json::nlohmann_json)
ament_target_dependencies(native_navigation_probe rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs)
ament_target_dependencies(native_navigation_probe rclcpp rclcpp_action bt_skill_interfaces navigation_interfaces geometry_msgs std_msgs)
''' % tuple(q(root / p) for p in (
'ros2/bt_executor/tools/native_navigation_probe.cpp',
'ros2/bt_executor/src/ros_driver.cpp', 'core/src/core.cpp',
@@ -60,21 +61,23 @@ def run(binary, output):
from rclpy.action import ActionClient, ActionServer, CancelResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from bt_skill_interfaces.action import Navigate
from bt_skill_interfaces.msg import NavigationResult
from navigation_interfaces.action import NavigateToPose as Navigate
from action_msgs.msg import GoalStatus
assert [getattr(NavigationResult, name) for name in
assert [getattr(Navigate.Result, name) for name in
('SUCCEEDED', 'CANCELED', 'TIMEOUT', 'BLOCKED', 'NOT_READY', 'FAILED')] == list(range(6))
assert [getattr(Navigate.Feedback, name) for name in
('ACCEPTED', 'CHECKING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(5))
('ACCEPTED', 'CHECKING', 'PLANNING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(6))
assert set(Navigate.Goal.get_fields_and_field_types()) == {
'trace', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'}
'task_id', 'subtask_id', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'}
assert set(Navigate.Result.get_fields_and_field_types()) == {
'result', 'final_pose_valid', 'final_pose', 'final_position_error', 'final_yaw_error'}
'status', 'error_code', 'message', 'final_pose_valid', 'final_pose',
'final_position_error', 'final_yaw_error', 'stop_state', 'stopped_at', 'stop_evidence_ref'}
assert set(Navigate.Feedback.get_fields_and_field_types()) == {
'stamp', 'sequence', 'phase', 'current_pose_valid', 'current_pose', 'error_valid',
'position_error', 'yaw_error', 'blocked', 'elapsed_time', 'message'}
'position_error', 'yaw_error', 'blocked_valid', 'blocked', 'elapsed_time', 'message'}
assert Navigate.Feedback.get_fields_and_field_types()['sequence'] == 'uint64'
assert (Navigate.Result.STOP_UNKNOWN, Navigate.Result.STOP_CONFIRMED) == (0, 1)
namespace = '/sim/navigation_contract_' + str(os.getpid())
rclpy.init()
node = Node('navigation_contract_fixture', namespace=namespace)
@@ -85,51 +88,51 @@ def run(binary, output):
def execute(handle):
goal = handle.request
scenario = round(goal.target_pose.pose.position.x)
status = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario]
status = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4, 9: 1, 10: 4}[scenario]
with lock:
counts[(goal.trace.task_id, scenario)] += 1
counts[(goal.task_id, scenario)] += 1
try:
assert goal.target_pose.header.frame_id == 'map'
assert goal.target_pose.pose.orientation.w == 1.
assert goal.position_tolerance == .05 and goal.yaw_tolerance == .1
assert goal.timeout.sec == 5
assert goal.trace.attempt == 1 and goal.trace.task_revision == 1
assert goal.trace.plan_version == 1 and goal.trace.execution_generation == 1
assert goal.task_id and goal.subtask_id == 'navigate'
except AssertionError:
failures.append('Goal contract mismatch: ' + str(goal))
for phase in range(5):
for phase in range(6):
feedback = Navigate.Feedback()
feedback.stamp = node.get_clock().now().to_msg()
feedback.sequence = phase + 1
feedback.sequence = (1 << 32) + phase + 1
feedback.phase = phase
feedback.current_pose_valid = True
feedback.current_pose = goal.target_pose
feedback.error_valid = True
feedback.position_error = .02
feedback.yaw_error = -.03
feedback.blocked_valid = phase != Navigate.Feedback.STOPPING
feedback.blocked = phase == Navigate.Feedback.BLOCKED
feedback.elapsed_time.nanosec = (phase + 1) * 10000000
feedback.message = 'phase-' + str(phase)
handle.publish_feedback(feedback)
time.sleep(.03)
if phase == 2 and goal.trace.task_id.startswith('probe-'):
if phase == 2 and goal.task_id.startswith('probe-'):
# A later malformed sample must not poison the accepted sequence.
wrong_frame = deepcopy(feedback)
wrong_frame.sequence = 100
wrong_frame.sequence = (1 << 32) + 100
wrong_frame.current_pose.header.frame_id = 'odom'
handle.publish_feedback(wrong_frame)
wrong_phase = deepcopy(feedback)
wrong_phase.sequence = 101
wrong_phase.sequence = (1 << 32) + 101
wrong_phase.phase = 255
handle.publish_feedback(wrong_phase)
result = Navigate.Result()
result.result.status = status
result.result.error_code = '' if scenario in (7, 8) else 'FIXTURE_' + str(scenario)
result.result.message = 'outcome-' + str(scenario)
result.result.stop_state = 0 if scenario == 6 else 1
result.result.stopped_at = node.get_clock().now().to_msg()
result.result.stop_evidence_ref = '' if scenario == 6 else 'sim://navigation/stop'
result.final_pose_valid = True
result.status = status
result.error_code = '' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario)
result.message = 'outcome-' + str(scenario)
result.stop_state = 0 if scenario in (6, 10) else 1
result.stopped_at = node.get_clock().now().to_msg()
result.stop_evidence_ref = '' if scenario in (6, 9, 10) else 'sim://navigation/stop'
result.final_pose_valid = scenario != 10
result.final_pose = goal.target_pose
result.final_position_error = .02
result.final_yaw_error = -.03
@@ -156,16 +159,14 @@ def run(binary, output):
executor.add_node(node)
thread = threading.Thread(target=executor.spin, daemon=True)
thread.start()
report = {'scope': 'simulation-only generated Navigate and production RosDriver over DDS',
report = {'scope': 'simulation-only generated NavigateToPose and production RosDriver over DDS',
'namespace': namespace, 'direct': [], 'native': []}
try:
assert client.wait_for_server(timeout_sec=10)
for scenario in range(9):
for scenario in range(11):
goal = Navigate.Goal()
goal.trace.task_id = 'direct-' + str(scenario)
goal.trace.run_id = goal.trace.task_id
goal.trace.subtask_id = 'navigate'
goal.trace.attempt = goal.trace.task_revision = goal.trace.plan_version = goal.trace.execution_generation = 1
goal.task_id = 'direct-' + str(scenario)
goal.subtask_id = 'navigate'
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = float(scenario)
goal.target_pose.pose.orientation.w = 1.
@@ -175,25 +176,26 @@ def run(binary, output):
feedback = []
handle = wait(client.send_goal_async(goal, feedback_callback=lambda value: feedback.append(value.feedback)))
assert handle.accepted
if scenario in (1, 6):
if scenario in (1, 6, 9):
assert wait(handle.cancel_goal_async()).return_code == 0
response = wait(handle.get_result_async())
expected = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario]
assert response.result.result.status == expected
expected = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4, 9: 1, 10: 4}[scenario]
assert response.result.status == expected
assert response.status == (GoalStatus.STATUS_SUCCEEDED if expected == 0 else
GoalStatus.STATUS_CANCELED if expected == 1 else GoalStatus.STATUS_ABORTED)
assert response.result.final_pose_valid
assert response.result.final_pose_valid == (scenario != 10)
assert response.result.final_pose.pose.position.x == float(scenario)
assert response.result.final_position_error == .02 and response.result.final_yaw_error == -.03
assert response.result.result.stop_state == (0 if scenario == 6 else 1)
assert response.result.result.error_code == ('' if scenario in (7, 8) else 'FIXTURE_' + str(scenario))
assert [f.phase for f in feedback] == list(range(5)), feedback
assert [f.sequence for f in feedback] == list(range(1, 6))
assert response.result.stop_state == (0 if scenario in (6, 10) else 1)
assert response.result.error_code == ('' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario))
assert [f.phase for f in feedback] == list(range(6)), feedback
assert [f.sequence for f in feedback] == [(1 << 32) + n for n in range(1, 7)]
assert all(f.current_pose_valid and f.error_valid and f.position_error == .02 and
f.yaw_error == -.03 and f.stamp.sec > 0 for f in feedback)
assert feedback[3].blocked and not feedback[4].blocked
assert feedback[4].blocked_valid and feedback[4].blocked
assert not feedback[5].blocked_valid and not feedback[5].blocked
report['direct'].append({'scenario': scenario, 'status': expected, 'feedback_phases': [f.phase for f in feedback]})
for scenario, expected_code in enumerate((0, 2, 3, 1, 4, 1, 2, 1, 4)):
for scenario, expected_code in enumerate((0, 2, 3, 1, 4, 1, 2, 1, 4, 2, 4)):
state = output / ('state-' + str(scenario))
state.mkdir(exist_ok=True)
completed = subprocess.run([str(binary), str(scenario), str(state), namespace],
@@ -208,19 +210,22 @@ def run(binary, output):
raise AssertionError('Native probe failed: ' + json.dumps(report['failure']))
result = json.loads(next(line for line in reversed(completed.stdout.splitlines()) if line.startswith('{')))
assert result['code'] == expected_code, result
assert result['stop'] == (0 if scenario == 6 else 1), result
assert result['state'] == (3 if scenario == 6 else 4), result
assert result['robot_locked'] == (scenario == 6), result
assert result['unknown_stop_blocks_redispatch'] == (scenario == 6), result
assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY'}.get(scenario, 'FIXTURE_' + str(scenario))), result
assert result['feedback_sequence'] == 5, result
assert json.loads(result['feedback'])['phase'] == 4, result
assert result['stop'] == (0 if scenario in (6, 9, 10) else 1), result
assert result['state'] == (3 if scenario in (6, 9, 10) else 4), result
assert result['robot_locked'] == (scenario in (6, 9, 10)), result
assert result['unknown_stop_blocks_redispatch'] == (scenario in (6, 9, 10)), result
assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY', 10: 'EXECUTION_BACKEND_NOT_CONFIGURED'}.get(scenario, 'FIXTURE_' + str(scenario))), result
assert result['feedback_sequence'] == (1 << 32) + 6, result
assert json.loads(result['feedback'])['phase'] == 5
assert json.loads(result['feedback'])['blocked_valid'] is False, result
assert json.loads(result['feedback'])['blocked'] is None, result
assert result['mapping_count'] == 1 and result['wire_result_bytes'] > 0, result
assert 'Navigate' in result['wire_request_type'] and 'Navigate' in result['wire_result_type'], result
assert result['response_valid'], result
assert result['wire_request_type'] == 'navigation_interfaces/action/NavigateToPose_Goal', result
assert result['wire_result_type'] == 'navigation_interfaces/action/NavigateToPose_Result', result
assert result['response_valid'] == (scenario != 10), result
report['native'].append(result)
assert not failures, failures
assert len(counts) == 18 and all(count == 1 for count in counts.values()), dict(counts)
assert len(counts) == 22 and all(count == 1 for count in counts.values()), dict(counts)
report['exactly_once_goal_count'] = sum(counts.values())
report['passed'] = True
(output / 'navigation-contract-report.json').write_text(json.dumps(report, indent=2) + '\n')
tests/helpers/native_workflow_regression.py
+23 -5
View File
@@ -94,6 +94,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
from rclpy.node import Node
from rclpy.serialization import deserialize_message
from bt_skill_interfaces.action import ExecuteTask, ExecuteManipulation
from navigation_interfaces.action import NavigateToPose
from bt_skill_interfaces.msg import RobotState
from bt_skill_interfaces.srv import ReconcileTask
from robot_bt_coordinator.plan_v2 import item_plan
@@ -105,7 +106,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
stops = ['shelf_A_stop', 'tote_A_stop'] if suffix == 'object_table' else ['observe_A', 'shelf_A_stop', 'tote_A_stop']
scenarios = {'verify_state': [{'kind': 'passed'}],
'locate_shelf_column': [{'kind': 'normal', 'shelf_id': 'shelf_A', 'side_id': 'FRONT', 'column_id': '1', 'tier_id': '2'}],
'navigate_semantic': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]}
'navigate': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]}
actions = [name for name in ACTION_TYPES if name != 'execute_task']
rclpy.init(args=['--ros-args', '-p', 'initial_holding_state:=EMPTY',
'-p', 'scenarios_json:=' + json.dumps(json.dumps(scenarios)),
@@ -178,7 +179,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
def motion_counts():
with server.lock:
return {name: server.counts[name] for name in ('navigate_semantic', 'execute_manipulation', 'execute_posture')}
return {name: server.counts[name] for name in ('navigate', 'execute_manipulation', 'execute_posture')}
try:
start()
@@ -195,6 +196,23 @@ def run_route(root, binary, output, suffix, recovery_enabled):
row['decoded_manipulation'] = assert_manipulation_evidence(
journal_records(Path(journal) / 'goal_registry.log'), goal.trace.run_id,
ExecuteManipulation, deserialize_message)
navigation_rows = [entry for entry in journal_records(Path(journal) / 'goal_registry.log')
if entry['run_id'] == goal.trace.run_id and entry['skill'] == 0]
assert len(navigation_rows) == len(stops)
expected_poses = {(site['locations'][name]['x'], site['locations'][name]['y']) for name in stops}
actual_poses = set()
for entry in navigation_rows:
assert entry['type'] == 'navigation_interfaces/action/NavigateToPose_Goal'
navigation_goal = deserialize_message(bytes.fromhex(entry['wire']), NavigateToPose.Goal)
assert navigation_goal.task_id == goal.trace.task_id and navigation_goal.subtask_id
assert navigation_goal.target_pose.header.frame_id == 'map'
actual_poses.add((navigation_goal.target_pose.pose.position.x, navigation_goal.target_pose.pose.position.y))
assert entry['result_type'] == 'navigation_interfaces/action/NavigateToPose_Result'
navigation_result = deserialize_message(bytes.fromhex(entry['result_wire']), NavigateToPose.Result)
assert navigation_result.status == NavigateToPose.Result.SUCCEEDED
assert navigation_result.stop_state == NavigateToPose.Result.STOP_CONFIRMED
assert actual_poses == expected_poses
row['direct_navigation_goals'] = len(navigation_rows)
assert motion_counts()['execute_manipulation'] == 2
if recovery_enabled:
stop()
@@ -228,11 +246,11 @@ def run_route(root, binary, output, suffix, recovery_enabled):
unknown_holding=unknown.error_code, recovered=state)
if suffix == 'object_table':
with server.lock:
server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}]
server.scenarios['navigate'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}]
canceled_goal = task_goal('cancel')
canceled_handle = wait(client.send_goal_async(canceled_goal), 8)
assert canceled_handle.accepted
eventually(lambda: motion_counts()['navigate_semantic'] > before['navigate_semantic'])
eventually(lambda: motion_counts()['navigate'] > before['navigate'])
wait(canceled_handle.cancel_goal_async(), 5)
canceled_result = wait(canceled_handle.get_result_async(), 15)
assert canceled_result.status == 5 and canceled_result.result.result.stop_state == 1
@@ -267,7 +285,7 @@ def run_route(root, binary, output, suffix, recovery_enabled):
if suffix == 'shelf_cell':
with server.lock:
server.scenarios['locate_shelf_column'] = [{'kind': 'ambiguous'}]
server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}]
server.scenarios['navigate'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}]
before_ask = motion_counts()
ask_handle = wait(client.send_goal_async(task_goal('ambiguous')), 8)
assert ask_handle.accepted
tests/helpers/ros_shim.py
+37 -33
View File
@@ -81,14 +81,16 @@ def _make_class(name, lines):
if kwargs:
raise TypeError("unexpected fields: " + ", ".join(kwargs))
attrs = {"__slots__": slots, "__init__": init, **constants}
attrs = {"__slots__": slots, "__init__": init, **constants,
"get_fields_and_field_types": classmethod(lambda cls: {field: kind for kind, field in fields})}
return type(name, (), attrs)
def install():
"""Install deterministic rclpy and generated interface modules."""
_registry.clear()
for name in list(sys.modules):
if name == "rclpy" or name.startswith("rclpy.") or name.startswith("bt_skill_interfaces"):
if name == "rclpy" or name.startswith("rclpy.") or name.startswith(("bt_skill_interfaces", "navigation_interfaces")):
del sys.modules[name]
builtin = _module("builtin_interfaces")
@@ -127,39 +129,41 @@ def install():
setattr(geometry_msg, name, cls)
_registry[f"geometry_msgs/{name}"] = cls
package = _module("bt_skill_interfaces")
msg_module = _module("bt_skill_interfaces.msg")
package.msg = msg_module
pending = {p.stem: _fields(p) for p in (IDL / "msg").glob("*.msg")}
while pending:
progress = False
for name, lines in list(pending.items()):
deps = [line.split()[0].removesuffix("[]") for line in lines if "=" not in line]
if all(dep in _registry or dep in ("string", "bool") or dep.startswith(("uint", "int", "float")) for dep in deps):
cls = _make_class(name, lines)
setattr(msg_module, name, cls)
_registry[f"bt_skill_interfaces/{name}"] = cls
del pending[name]
progress = True
if not progress:
raise RuntimeError("unresolved IDL: " + repr(pending))
for package_name in ("bt_skill_interfaces", "navigation_interfaces"):
package_idl = IDL.parent / package_name
package = _module(package_name)
msg_module = _module(f"{package_name}.msg")
package.msg = msg_module
pending = {p.stem: _fields(p) for p in (package_idl / "msg").glob("*.msg")}
while pending:
progress = False
for name, lines in list(pending.items()):
deps = [line.split()[0].removesuffix("[]") for line in lines if "=" not in line]
if all(dep in _registry or dep in ("string", "bool") or dep.startswith(("uint", "int", "float")) for dep in deps):
cls = _make_class(name, lines)
setattr(msg_module, name, cls)
_registry[f"{package_name}/{name}"] = cls
del pending[name]
progress = True
if not progress:
raise RuntimeError("unresolved IDL: " + repr(pending))
action_module = _module("bt_skill_interfaces.action")
package.action = action_module
for path in (IDL / "action").glob("*.action"):
action = type(path.stem, (), {})
action.Goal = _make_class("Goal", _fields(path, 0))
action.Result = _make_class("Result", _fields(path, 1))
action.Feedback = _make_class("Feedback", _fields(path, 2))
setattr(action_module, path.stem, action)
action_module = _module(f"{package_name}.action")
package.action = action_module
for path in (package_idl / "action").glob("*.action"):
action = type(path.stem, (), {})
action.Goal = _make_class("Goal", _fields(path, 0))
action.Result = _make_class("Result", _fields(path, 1))
action.Feedback = _make_class("Feedback", _fields(path, 2))
setattr(action_module, path.stem, action)
srv_module = _module("bt_skill_interfaces.srv")
package.srv = srv_module
for path in (IDL / "srv").glob("*.srv"):
srv = type(path.stem, (), {})
srv.Request = _make_class("Request", _fields(path, 0))
srv.Response = _make_class("Response", _fields(path, 1))
setattr(srv_module, path.stem, srv)
srv_module = _module(f"{package_name}.srv")
package.srv = srv_module
for path in (package_idl / "srv").glob("*.srv"):
srv = type(path.stem, (), {})
srv.Request = _make_class("Request", _fields(path, 0))
srv.Response = _make_class("Response", _fields(path, 1))
setattr(srv_module, path.stem, srv)
rclpy = _module("rclpy")
rclpy.ok = lambda: True