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

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros2/bt_mock_servers/bt_mock_servers/mock_skills.py
+53 -25
View File
@@ -21,7 +21,7 @@ from bt_skill_interfaces.action import (
EvaluateProgress, ExecuteTask, LocalizeTarget3D, LocateShelfColumn,
Navigate, NavigateSemantic, PlanTask, VerifyState,
)
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, RobotState, SafetyState,
from bt_skill_interfaces.msg import (DenseProgress, ExecutionResult, NavigationResult, RobotState, SafetyState,
VerificationEvidence, VisualObservation)
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
from std_msgs.msg import String
@@ -43,8 +43,8 @@ ACTION_ENDPOINTS = {
}
MOTION = frozenset(("navigate", "navigate_semantic", "execute_manipulation", "execute_posture", "execute_task"))
SUCCESS_PHASES = {
"navigate": (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
Navigate.Feedback.NAVIGATING, Navigate.Feedback.ARRIVING),
"navigate": (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING),
"execute_manipulation": (ExecuteManipulation.Feedback.PREPARING,
ExecuteManipulation.Feedback.WAITING_OBSERVATION,
ExecuteManipulation.Feedback.INFERRING,
@@ -68,11 +68,25 @@ def elapsed_message(seconds):
return Duration(sec=nanoseconds // 1000000000, nanosec=nanoseconds % 1000000000)
def normalized_navigation_pose(target_pose):
pose = copy.deepcopy(target_pose)
q = pose.pose.orientation
scale = max(abs(q.x), abs(q.y), abs(q.z), abs(q.w))
values = [value / scale for value in (q.x, q.y, q.z, q.w)]
norm = math.sqrt(sum(value * value for value in values))
q.x, q.y, q.z, q.w = (value / norm for value in values)
return pose
def lifecycle_phases(name, kind):
if name == "navigate" and kind == "obstacle_recovery":
return (Navigate.Feedback.CHECKING, Navigate.Feedback.PLANNING,
Navigate.Feedback.NAVIGATING, Navigate.Feedback.WAITING_OBSTACLE,
Navigate.Feedback.RECOVERING, Navigate.Feedback.ARRIVING)
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING,
Navigate.Feedback.NAVIGATING, Navigate.Feedback.BLOCKED,
Navigate.Feedback.NAVIGATING)
if name == "navigate" and kind == "blocked":
return (*SUCCESS_PHASES[name], Navigate.Feedback.BLOCKED)
if name == "navigate" and kind == "not_ready":
return (Navigate.Feedback.ACCEPTED, Navigate.Feedback.CHECKING)
return SUCCESS_PHASES.get(name, (0,))
@@ -170,13 +184,13 @@ class MockSkills(Node):
raise ValueError("capture boundary is in the future")
if name == "navigate":
p, q = request.target_pose.pose.position, request.target_pose.pose.orientation
values = [p.x, p.y, p.z, q.x, q.y, q.z, q.w, request.position_tolerance, request.orientation_tolerance]
if not all(math.isfinite(v) for v in values) or not request.target_pose.header.frame_id:
values = [p.x, p.y, p.z, q.x, q.y, q.z, q.w, request.position_tolerance, request.yaw_tolerance]
if not all(math.isfinite(v) for v in values) or request.target_pose.header.frame_id != "map":
raise ValueError("navigation pose is invalid")
if request.position_tolerance <= 0 or not 0 < request.orientation_tolerance <= math.pi:
if request.position_tolerance <= 0 or request.yaw_tolerance <= 0:
raise ValueError("navigation tolerances are invalid")
if abs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1.0) > 0.001:
raise ValueError("navigation quaternion must have unit norm")
if not any((q.x, q.y, q.z, q.w)):
raise ValueError("navigation quaternion must be nonzero")
elif name == "execute_manipulation":
if request.skill not in ("pick", "place") or not request.instruction.strip():
raise ValueError("manipulation skill/instruction is invalid")
@@ -310,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" and outcome == ExecutionResult.COMPLETED:
if (kind == "failed" or name == "navigate" and kind in ("blocked", "not_ready")) and outcome == ExecutionResult.COMPLETED:
outcome = ExecutionResult.FAILED
if kind == "stop_unknown" and outcome == ExecutionResult.COMPLETED:
outcome, stop_state = ExecutionResult.FAILED, ExecutionResult.UNKNOWN
@@ -320,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))
result.result = self._execution_result(ExecutionResult.FAILED, stop_state, goal_id, "MOCK_EXCEPTION", str(exc), name=name)
elif hasattr(result, "evidence"):
result.evidence.status = VerificationEvidence.UNKNOWN
result.evidence.error_code = "MOCK_EXCEPTION"
@@ -371,13 +385,12 @@ class MockSkills(Node):
if hasattr(feedback, "elapsed_time"):
feedback.elapsed_time = elapsed_message(elapsed)
if name == "navigate":
feedback.pose_valid = True
feedback.current_pose = copy.deepcopy(handle.request.target_pose)
feedback.errors_valid = True
feedback.current_pose_valid = True
feedback.current_pose = normalized_navigation_pose(handle.request.target_pose)
feedback.error_valid = True
feedback.position_error = 0.0
feedback.orientation_error = 0.0
feedback.blocked_valid = True
feedback.blocked = False
feedback.yaw_error = 0.0
feedback.blocked = feedback.phase == Navigate.Feedback.BLOCKED
if name == "execute_manipulation":
feedback.progress_valid = False
handle.publish_feedback(feedback)
@@ -389,9 +402,18 @@ class MockSkills(Node):
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"):
result = ExecutionResult()
result.status, result.stop_state = outcome, stop_state
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
result.error_code = error or ("" if outcome == ExecutionResult.COMPLETED else "MOCK_TERMINATED")
result.message = message
if stop_state == ExecutionResult.CONFIRMED:
@@ -408,14 +430,20 @@ class MockSkills(Node):
record = "sim://" + name + "/" + goal_id
ok = outcome == ExecutionResult.COMPLETED
if hasattr(result, "result"):
result.result = self._execution_result(outcome, stop_state, goal_id)
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.pose_valid = result.errors_valid = ok
result.final_pose = copy.deepcopy(request.target_pose)
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
ros2/bt_mock_servers/bt_mock_servers/scenarios.py
+9 -3
View File
@@ -17,7 +17,7 @@ KINDS = {
"unavailable", "invalid_pose", "emergency_stop", "protective_stop",
"model_estimate", "fused",
"cancel_stop_unknown", "timeout_stop_unknown",
"obstacle_recovery",
"obstacle_recovery", "blocked", "not_ready",
}
FIXTURE_FIELDS = {
"kind", "duration_seconds", "shelf_id", "side_id", "column_id", "tier_id",
@@ -26,12 +26,12 @@ FIXTURE_FIELDS = {
"observation_id", "image_path", "station_id", "registry_version",
"calibration_id", "geometry_epoch", "status_json",
"stop_delay_seconds",
"final_pose",
"final_pose", "error_code",
}
BASE_KINDS = {"normal", "failed", "timeout", "silence", "reject", "native_mismatch"}
ACTION_KINDS = {
"navigate": BASE_KINDS | {"stop_unknown", "cancel_stop_unknown", "timeout_stop_unknown", "obstacle_recovery"},
"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"},
@@ -86,6 +86,12 @@ def parse_scenarios(raw):
raise ValueError("unsupported fixture kind")
if fixture.get("kind", "normal") not in ACTION_KINDS[name]:
raise ValueError("fixture kind has no effect for " + name)
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",
"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)
if type(delay) not in (int, float) or not math.isfinite(delay) or not 0 <= delay <= 120:
raise ValueError("fixture duration must be finite in [0,120]")