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
navigation_gateway/semantic_proxy.py
+38 -8
View File
@@ -9,6 +9,31 @@ def duration_seconds(value):
if not 0<seconds<=3600:raise ValueError('Duration must be in (0,3600] seconds')
return seconds
def translate_navigation_result(source, target, navigation_enum, execution_enum):
"""Preserve proof and diagnostics while explicitly translating different enums."""
mapping = {navigation_enum.SUCCEEDED: execution_enum.COMPLETED,
navigation_enum.CANCELED: execution_enum.CANCELED,
navigation_enum.TIMEOUT: execution_enum.TIMED_OUT,
navigation_enum.BLOCKED: execution_enum.FAILED,
navigation_enum.NOT_READY: execution_enum.REJECTED,
navigation_enum.FAILED: execution_enum.FAILED}
target.result.status = mapping[source.result.status]
target.result.stop_state = {navigation_enum.UNKNOWN: execution_enum.UNKNOWN,
navigation_enum.CONFIRMED: execution_enum.CONFIRMED}[source.result.stop_state]
for field in ('error_code', 'message', 'stopped_at', 'stop_evidence_ref'):
setattr(target.result, field, getattr(source.result, field))
if not target.result.error_code:
target.result.error_code = {navigation_enum.BLOCKED: 'NAV_BLOCKED',
navigation_enum.NOT_READY: 'NAV_NOT_READY'}.get(source.result.status, '')
target.final_pose = source.final_pose
target.pose_valid = source.final_pose_valid
target.errors_valid = (source.final_pose_valid and
all(math.isfinite(v) for v in (source.final_position_error, source.final_yaw_error)) and
source.final_position_error >= 0 and abs(source.final_yaw_error) <= math.pi)
target.final_position_error = source.final_position_error
target.final_orientation_error = source.final_yaw_error
def main():
import rclpy
from rclpy.node import Node
@@ -16,6 +41,8 @@ def main():
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from bt_skill_interfaces.action import Navigate,NavigateSemantic
from bt_skill_interfaces.msg import NavigationResult,ExecutionResult
from action_msgs.msg import GoalStatus
class Proxy(Node):
def __init__(self):
super().__init__('navigation_semantic_proxy');self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.reserved=False;self.faulted=False
@@ -38,7 +65,7 @@ def main():
try:
g=h.request;lookup=self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
deadline=self.accepted_at+duration_seconds(g.timeout)
q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.orientation_tolerance=g.orientation_tolerance
q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.yaw_tolerance=g.orientation_tolerance
p=lookup['pose'];q.target_pose.header.frame_id=p['frame_id'];q.target_pose.header.stamp=self.get_clock().now().to_msg()
for k in ('x','y','z'):setattr(q.target_pose.pose.position,k,float(p[k]))
for k in ('x','y','z','w'):setattr(q.target_pose.pose.orientation,k,float(p['q'+k]))
@@ -46,7 +73,12 @@ def main():
remaining_ns=int((deadline-time.monotonic())*1e9)
if remaining_ns<=0 or h.is_cancel_requested:raise RuntimeError('EXPIRED_BEFORE_DISPATCH')
q.timeout.sec=remaining_ns//1_000_000_000;q.timeout.nanosec=remaining_ns%1_000_000_000
future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0.
def forward_feedback(event):
source=event.feedback
f=NavigateSemantic.Feedback()
for field in ('stamp','sequence','phase','message'):setattr(f,field,getattr(source,field))
h.publish_feedback(f)
future=self.client.send_goal_async(q,feedback_callback=forward_feedback);acceptance_deadline=min(deadline,time.monotonic()+3.)
while not future.done():
if h.is_cancel_requested or time.monotonic()>acceptance_deadline:
def late(f):
@@ -63,20 +95,18 @@ def main():
now=time.monotonic()
if (h.is_cancel_requested or now>=deadline) and cancel_at is None:downstream.cancel_goal_async();cancel_at=now
if cancel_at is not None and now-cancel_at>5:raise RuntimeError('STOP_UNKNOWN')
if now-last_feedback>=.2:
sequence+=1;f=NavigateSemantic.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=2 if cancel_at else 1;f.message='awaiting downstream result';h.publish_feedback(f);last_feedback=now
time.sleep(.01)
wrapped=done.result();m=wrapped.result
for field in ('result','final_pose','pose_valid','final_position_error','final_orientation_error','errors_valid'):setattr(result,field,getattr(m,field))
if wrapped.status==4 and m.result.status==0:h.succeed()
elif wrapped.status==5 and h.is_cancel_requested:h.canceled()
translate_navigation_result(m,result,NavigationResult,ExecutionResult)
if wrapped.status==GoalStatus.STATUS_SUCCEEDED and m.result.status==NavigationResult.SUCCEEDED:h.succeed()
elif wrapped.status==GoalStatus.STATUS_CANCELED and m.result.status==NavigationResult.CANCELED and h.is_cancel_requested:h.canceled()
else:h.abort()
except Exception as ex:
self.faulted=True
if downstream is not None and downstream.accepted:
try:downstream.cancel_goal_async()
except Exception:pass
result.result.status=1;result.result.stop_state=0;result.result.error_code=str(ex);h.abort()
result.result.status=ExecutionResult.FAILED;result.result.stop_state=ExecutionResult.UNKNOWN;result.result.error_code=str(ex);h.abort()
finally:
with self.lock:self.reserved=False
return result