behavior-tree/navigation_gateway/semantic_proxy.py

"""ROS2 lookup facade. Existing Navigate proxy still owns physical stop proof."""
import json,threading,time,math
from .catalog import Catalog

def duration_seconds(value):
    if type(value.sec) is not int or type(value.nanosec) is not int or value.sec<0 or not 0<=value.nanosec<1_000_000_000:
        raise ValueError('invalid normalized Duration')
    seconds=value.sec+value.nanosec/1e9
    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
    from rclpy.action import ActionServer,ActionClient,GoalResponse,CancelResponse
    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
            path=self.declare_parameter('site_config_file','').value
            with open(path) as f:self.catalog=Catalog(json.load(f))
            self.client=ActionClient(self,Navigate,self.declare_parameter('navigate_action','skills/navigate').value,callback_group=self.group)
            self.server=ActionServer(self,NavigateSemantic,'skills/navigate_semantic',execute_callback=self.execute,goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group)
        def admit(self,g):
            try:
                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)
                duration_seconds(g.timeout)
                if any(not math.isfinite(x) or x<=0 for x in (g.position_tolerance,g.orientation_tolerance)) or g.orientation_tolerance>math.pi:return GoalResponse.REJECT
                with self.lock:
                    if self.reserved or self.faulted:return GoalResponse.REJECT
                    self.reserved=True;self.accepted_at=time.monotonic()
                return GoalResponse.ACCEPT
            except (ValueError,KeyError):return GoalResponse.REJECT
        def execute(self,h):
            result=NavigateSemantic.Result();downstream=None;future=None
            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.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]))
                if not self.client.server_is_ready():raise RuntimeError('NAVIGATE_UNAVAILABLE')
                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
                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):
                            try:
                                accepted=f.result()
                                if accepted.accepted:accepted.cancel_goal_async()
                            except Exception:pass
                        future.add_done_callback(late);raise RuntimeError('ACCEPTANCE_UNKNOWN')
                    time.sleep(.01)
                downstream=future.result()
                if not downstream.accepted:raise RuntimeError('NAVIGATE_REJECTED')
                done=downstream.get_result_async();cancel_at=None
                while not done.done():
                    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')
                    time.sleep(.01)
                wrapped=done.result();m=wrapped.result
                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=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
    rclpy.init();node=Proxy();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node)
    try:executor.spin()
    finally:executor.shutdown();node.destroy_node();rclpy.shutdown()
if __name__=='__main__':main()