87 lines
5.8 KiB
Python
87 lines
5.8 KiB
Python
|
"""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 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
|
||
|
|
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.orientation_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
|
||
|
|
future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0.
|
||
|
|
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')
|
||
|
|
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()
|
||
|
|
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()
|
||
|
|
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()
|