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
ros2/bt_mock_servers/bt_mock_servers/mock_skills.py
+139 -69
View File
@@ -19,20 +19,21 @@ from rclpy.node import Node
from bt_skill_interfaces.action import (
AssessGrasp, CheckFreeSpace, ExecuteManipulation, ExecutePosture,
EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn,
Navigate, NavigateSemantic, PlanTask, VerifyState,
PlanTask, VerifyState,
)
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, NavigationResult, RobotState, SafetyState,
from navigation_interfaces.action import NavigateToPose
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState,
VerificationEvidence, VisualObservation)
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
from std_msgs.msg import String
from .scenarios import duration_seconds, fixture_at, fixed_plan, parse_scenarios, strict_json, validate_trace
ACTION_TYPES = {
"navigate": Navigate, "execute_manipulation": ExecuteManipulation,
"navigate": NavigateToPose, "execute_manipulation": ExecuteManipulation,
"plan_task": PlanTask, "locate_shelf_column": LocateShelfColumn,
"localize_target_3d": LocalizeTarget3D, "check_free_space": CheckFreeSpace,
"assess_grasp": AssessGrasp, "execute_posture": ExecutePosture, "verify_state": VerifyState,
"navigate_semantic": NavigateSemantic, "evaluate_progress": EvaluateProgress,
"evaluate_progress": EvaluateProgress,
"execute_task": ExecuteTask,
}
ACTION_ENDPOINTS = {
@@ -41,10 +42,10 @@ ACTION_ENDPOINTS = {
"execute_task": "tasks/execute",
"evaluate_progress": "monitor/evaluate_progress",
}
MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task"))
MOTION = frozenset(("navigate", "execute_manipulation", "execute_posture", "execute_task"))
SUCCESS_PHASES = {
"navigate": (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING),
"navigate": (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING,
NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING),
"execute_manipulation": (ExecuteManipulation.Feedback.PREPARING,
ExecuteManipulation.Feedback.WAITING_OBSERVATION,
ExecuteManipulation.Feedback.INFERRING,
@@ -55,7 +56,7 @@ SUCCESS_PHASES = {
"assess_grasp": (0,), "execute_posture": (ExecutePosture.Feedback.CHECKING,
ExecutePosture.Feedback.MOVING,
ExecutePosture.Feedback.SETTLING),
"verify_state": (0,), "navigate_semantic": (0,), "evaluate_progress": (0,),
"verify_state": (0,), "evaluate_progress": (0,),
}
@@ -80,13 +81,13 @@ def normalized_navigation_pose(target_pose):
def lifecycle_phases(name, kind):
if name == "navigate" and kind == "obstacle_recovery":
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING, Navigate.Feedback.BLOCKED,
Navigate.Feedback.NAVIGATING)
return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING,
NavigateToPose.Feedback.PLANNING, NavigateToPose.Feedback.NAVIGATING, NavigateToPose.Feedback.BLOCKED,
NavigateToPose.Feedback.NAVIGATING)
if name == "navigate" and kind == "blocked":
return (*SUCCESS_PHASES[name], Navigate.Feedback.BLOCKED)
return (*SUCCESS_PHASES[name], NavigateToPose.Feedback.BLOCKED)
if name == "navigate" and kind == "not_ready":
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING)
return (NavigateToPose.Feedback.ACCEPTED, NavigateToPose.Feedback.CHECKING)
return SUCCESS_PHASES.get(name, (0,))
@@ -199,15 +200,6 @@ class MockSkills(Node):
destination = (bool(request.destination.region_ref), bool(request.destination.description))
if destination != ((False, False) if request.skill == "pick" else (True, True)):
raise ValueError("destination must be empty for pick and complete for place")
elif name == "navigate_semantic":
if request.kind not in ("LOCATION", "OBJECT", "CELL") or not request.reference or not request.registry_version:
raise ValueError("semantic navigation binding is invalid")
if request.kind == "CELL" and any(
not isinstance(value, str) or not value or "/" in value
for value in (request.shelf_id, request.side_id, request.column_id, request.tier_id)):
raise ValueError("semantic cell binding is incomplete")
if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
raise ValueError("semantic navigation tolerances are invalid")
elif name == "execute_posture":
if request.posture_id not in self.allowed_postures or not request.expected_geometry_epoch:
raise ValueError("posture or geometry epoch is invalid")
@@ -281,10 +273,18 @@ class MockSkills(Node):
self.counts[name] += 1
self.accepted[goal_id] = (time.monotonic(), fixture)
if name in MOTION:
self.motion_owner = (goal_id, name, copy.deepcopy(handle.request.trace))
self.motion_owner = (goal_id, name, self._goal_identity(name, handle.request))
handle.execute()
@staticmethod
def _goal_identity(name, request):
if name == "navigate":
return (request.task_id, request.subtask_id)
return copy.deepcopy(request.trace)
def _execute(self, name, handle):
if name == "navigate":
return self._execute_navigation(handle)
goal_id = bytes(handle.goal_id.uuid).hex()
with self.lock:
started, fixture = self.accepted.pop(goal_id)
@@ -324,7 +324,7 @@ class MockSkills(Node):
self._feedback(name, handle, sequence, now - started, phases[phase_index])
emitted_phase = phase_index
time.sleep(min(0.02, max(0, deadline - now)))
if (kind == "failed" or name == "navigate" and kind in ("blocked", "not_ready")) and outcome == ExecutionResult.COMPLETED:
if kind == "failed" and outcome == ExecutionResult.COMPLETED:
outcome = ExecutionResult.FAILED
if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED:
outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN
@@ -334,7 +334,7 @@ class MockSkills(Node):
# Unknown execution state stays reserved. A client cannot infer stop from this exception.
stop_state = ExecutionResult.UNKNOWN if name in MOTION else ExecutionResult.CONFIRMED
if hasattr(result, "result"):
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc), name=name)
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc))
elif hasattr(result, "evidence"):
result.evidence.status = VerificationEvidence.UNKNOWN
result.evidence.error_code = "MOCK_EXCEPTION"
@@ -357,7 +357,109 @@ class MockSkills(Node):
if self.motion_owner and self.motion_owner[0] == goal_id:
self.motion_owner = None
elif name in MOTION:
self.unresolved_motion[goal_id] = (name, copy.deepcopy(handle.request.trace))
self.unresolved_motion[goal_id] = (name, self._goal_identity(name, handle.request))
return result
def _execute_navigation(self, handle):
"""Explicit simulator backend producing only native navigation messages."""
goal_id = bytes(handle.goal_id.uuid).hex()
with self.lock:
started, fixture = self.accepted.pop(goal_id)
kind = fixture.get("kind", "normal")
deadline = started + duration_seconds(handle.request.timeout)
finish = started + fixture.get("duration_seconds", 0.2)
result = NavigateToPose.Result()
result.status = result.SUCCEEDED
result.stop_state = result.STOP_UNKNOWN
sequence, emitted_phase = 0, -1
try:
while True:
now = time.monotonic()
if handle.is_cancel_requested:
result.status = result.CANCELED
self._stopping_feedback("navigate", handle, sequence + 1, now - started)
time.sleep(fixture.get("stop_delay_seconds", 0.0))
break
if now >= deadline or not rclpy.ok():
result.status = result.TIMEOUT
self._stopping_feedback("navigate", handle, sequence + 1, now - started)
break
if now >= finish and kind not in ("timeout", "timeout_stop_unknown", "silence"):
break
if kind != "silence":
phases = lifecycle_phases("navigate", kind)
phase_index = min(len(phases) - 1, int((now - started) / max(0.001, finish - started) * len(phases)))
if phase_index > emitted_phase:
sequence += 1
self._feedback("navigate", handle, sequence, now - started, phases[phase_index])
emitted_phase = phase_index
time.sleep(min(0.02, max(0, deadline - now)))
if result.status == result.SUCCEEDED:
if kind in ("failed", "stop_unknown"):
result.status = result.FAILED
elif kind == "blocked":
result.status = result.BLOCKED
elif kind == "not_ready":
result.status = result.NOT_READY
if kind not in ("stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"):
# This dedicated simulator explicitly observes its own stopped state.
result.stop_state = result.STOP_CONFIRMED
result.stopped_at = self.get_clock().now().to_msg()
result.stop_evidence_ref = "sim://stop/" + goal_id
result.error_code = "" if result.status == result.SUCCEEDED else "MOCK_TERMINATED"
if result.status == result.BLOCKED:
result.error_code = "BLOCKED"
elif result.status == result.NOT_READY:
result.error_code = fixture.get("error_code", "INPUTS_UNHEALTHY")
result.message = "SIMULATED navigation only"
result.final_pose_valid = result.status == result.SUCCEEDED
if result.final_pose_valid:
result.final_pose = normalized_navigation_pose(handle.request.target_pose)
result.final_pose.header.stamp = self.get_clock().now().to_msg()
pose_fixture = fixture.get("final_pose")
if pose_fixture is not None:
result.final_pose.header.frame_id = pose_fixture["frame_id"]
for field in ("x", "y", "z"):
setattr(result.final_pose.pose.position, field, float(pose_fixture[field]))
for field in ("x", "y", "z", "w"):
setattr(result.final_pose.pose.orientation, field, float(pose_fixture["q" + field]))
actual, target = result.final_pose.pose, normalized_navigation_pose(handle.request.target_pose).pose
result.final_position_error = math.hypot(actual.position.x - target.position.x,
actual.position.y - target.position.y,
actual.position.z - target.position.z)
def yaw(q):
return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
delta = yaw(target.orientation) - yaw(actual.orientation)
result.final_yaw_error = math.atan2(math.sin(delta), math.cos(delta))
if result.stop_state == result.STOP_CONFIRMED:
with self.lock:
self.geometry_epoch += 1
if kind == "native_mismatch":
handle.abort()
elif result.status == result.CANCELED:
handle.canceled()
elif result.status == result.SUCCEEDED:
handle.succeed()
else:
handle.abort()
except Exception as exc:
result.status, result.stop_state = result.FAILED, result.STOP_UNKNOWN
result.error_code, result.message = "MOCK_EXCEPTION", str(exc)
result.stopped_at, result.stop_evidence_ref = Time(), ""
result.final_pose_valid = False
if handle.is_active:
handle.abort()
self.get_logger().error("Mock navigation exception: " + str(exc))
finally:
with self.lock:
self.inflight -= 1
if result.stop_state == result.STOP_CONFIRMED:
self.motion_reserved = False
self.unresolved_motion.pop(goal_id, None)
if self.motion_owner and self.motion_owner[0] == goal_id:
self.motion_owner = None
else:
self.unresolved_motion[goal_id] = ("navigate", self._goal_identity("navigate", handle.request))
return result
@staticmethod
@@ -390,30 +492,22 @@ class MockSkills(Node):
feedback.error_valid = True
feedback.position_error = 0.0
feedback.yaw_error = 0.0
feedback.blocked = feedback.phase == Navigate.Feedback.BLOCKED
feedback.blocked_valid = True
feedback.blocked = feedback.phase == NavigateToPose.Feedback.BLOCKED
if name == "execute_manipulation":
feedback.progress_valid = False
handle.publish_feedback(feedback)
def _stopping_feedback(self, name, handle, sequence, elapsed):
stopping = {"navigate": Navigate.Feedback.STOPPING,
stopping = {"navigate": NavigateToPose.Feedback.STOPPING,
"execute_manipulation": ExecuteManipulation.Feedback.STOPPING,
"execute_posture": ExecutePosture.Feedback.STOPPING}.get(name)
if stopping is not None:
self._feedback(name, handle, sequence, elapsed, stopping)
def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only", name="", kind="normal"):
result = NavigationResult() if name == "navigate" else ExecutionResult()
if name == "navigate":
status = {ExecutionResult.COMPLETED: NavigationResult.SUCCEEDED,
ExecutionResult.CANCELED: NavigationResult.CANCELED,
ExecutionResult.TIMED_OUT: NavigationResult.TIMEOUT,
ExecutionResult.FAILED: NavigationResult.FAILED}[outcome]
if outcome == ExecutionResult.FAILED and kind in ("blocked", "not_ready"):
status = NavigationResult.BLOCKED if kind == "blocked" else NavigationResult.NOT_READY
else:
status = outcome
result.status, result.stop_state = status, stop_state
def _execution_result(self, outcome, stop_state, goal_id, error="", message="SIMULATED execution only"):
result = ExecutionResult()
result.status, result.stop_state = outcome, stop_state
result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED")
result.message = message
if stop_state == ExecutionResult.CONFIRMED:
@@ -430,34 +524,8 @@ class MockSkills(Node):
record = "sim://" + name + "/" + goal_id
ok = outcome == ExecutionResult.COMPLETED
if hasattr(result, "result"):
error = ""
if name == "navigate" and outcome == ExecutionResult.FAILED:
if kind == "blocked": error = "BLOCKED"
if kind == "not_ready": error = fixture.get("error_code", "INPUTS_UNHEALTHY")
result.result = self._execution_result(outcome, stop_state, goal_id, error, name=name, kind=kind)
if name in ("navigate", "navigate_semantic"):
if ok and stop_state == ExecutionResult.CONFIRMED:
with self.lock:
self.geometry_epoch += 1
if name == "navigate":
result.final_pose_valid = ok
if ok:
result.final_pose = normalized_navigation_pose(request.target_pose)
result.final_position_error = result.final_yaw_error = 0.0
else:
pose = fixture.get("final_pose")
result.pose_valid = result.errors_valid = ok and pose is not None
if pose is not None:
result.final_pose.header.frame_id = pose["frame_id"]
result.final_pose.pose.position.x = pose["x"]
result.final_pose.pose.position.y = pose["y"]
result.final_pose.pose.position.z = pose["z"]
result.final_pose.pose.orientation.x = pose["qx"]
result.final_pose.pose.orientation.y = pose["qy"]
result.final_pose.pose.orientation.z = pose["qz"]
result.final_pose.pose.orientation.w = pose["qw"]
result.final_pose.header.stamp = observed
elif name == "execute_manipulation":
result.result = self._execution_result(outcome, stop_state, goal_id)
if name == "execute_manipulation":
result.execution_record_ref = record
# Deliberately no holding/verification state mutation here.
elif name == "plan_task":
@@ -725,10 +793,12 @@ class MockSkills(Node):
fresh = 0 < observed_ns <= now < valid_until_ns
with self.lock:
owned = self.unresolved_motion.get(request.goal_id)
owned_trace_matches = owned is not None and all(
owned_trace_matches = owned is not None and owned[0] != "navigate" and all(
getattr(owned[1], field) == getattr(request.trace, field)
for field in ("task_id", "subtask_id", "attempt", "task_revision",
"plan_version", "run_id", "execution_generation"))
if owned is not None and owned[0] == "navigate":
owned_trace_matches = owned[1] == (request.trace.task_id, request.trace.subtask_id)
owner_matches = self.motion_owner is not None and self.motion_owner[0] == request.goal_id
bound = (owner_matches and owned_trace_matches and evidence.context.source_goal_id == request.goal_id and
same_trace and evidence.status == evidence.PASSED and
ros2/bt_mock_servers/bt_mock_servers/scenarios.py
+5 -6
View File
@@ -6,7 +6,7 @@ ACTION_NAMES = (
"navigate", "execute_manipulation", "plan_task", "locate_shelf_column",
"localize_target_3d", "check_free_space", "assess_grasp", "execute_posture",
"verify_state",
"navigate_semantic", "evaluate_progress", "execute_task",
"evaluate_progress", "execute_task",
"robot_state", "safety_state", "visual_observation", "dense_progress", "goal_registry",
)
KINDS = {
@@ -32,7 +32,6 @@ FIXTURE_FIELDS = {
BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"}
ACTION_KINDS = {
"navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery", "blocked", "not_ready"},
"navigate_semantic": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_manipulation": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_posture": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
"execute_task": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown"},
@@ -89,7 +88,7 @@ def parse_scenarios(raw):
if "error_code" in fixture:
code = fixture["error_code"]
if (name != "navigate" or fixture.get("kind") != "not_ready" or
code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
code not in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "EXECUTION_BACKEND_NOT_CONFIGURED",
"ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED"}):
raise ValueError("error_code requires a supported navigation readiness reason")
delay = fixture.get("duration_seconds", 0.2)
@@ -115,12 +114,12 @@ def parse_scenarios(raw):
if "final_pose" in fixture:
pose = fixture["final_pose"]
required = {"frame_id", "x", "y", "z", "qx", "qy", "qz", "qw"}
if not isinstance(pose, dict) or set(pose) != required or not isinstance(pose["frame_id"], str) or not pose["frame_id"]:
raise ValueError("final_pose requires an exact frame and pose")
if name != "navigate" or not isinstance(pose, dict) or set(pose) != required or pose["frame_id"] != "map":
raise ValueError("final_pose requires a navigation pose in map")
values = [pose[key] for key in ("x", "y", "z", "qx", "qy", "qz", "qw")]
if any(type(value) not in (int, float) or not math.isfinite(value) for value in values):
raise ValueError("final_pose values must be finite")
if abs(sum(pose[key] ** 2 for key in ("qx", "qy", "qz", "qw")) - 1.0) > 0.001:
if abs(math.hypot(*(pose[key] for key in ("qx", "qy", "qz", "qw"))) - 1.0) > 0.001:
raise ValueError("final_pose quaternion must have unit norm")
result[name] = entries
return result
ros2/bt_mock_servers/package.xml
+1
View File
@@ -8,6 +8,7 @@
<buildtool_depend>ament_python</buildtool_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>bt_skill_interfaces</exec_depend>
<exec_depend>navigation_interfaces</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>std_msgs</exec_depend>
<export><build_type>ament_python</build_type></export>