实现行为树执行器、任务协调和技能接口

ros2/robobrain_services/package.xml

@@ -0,0 +1,2 @@
+<?xml version="1.2.0"?>
+<package format="3"><name>robobrain_services</name><version>1.2.0</version><description>RoboBrain planning/spatial and RoboDopamine advisory services</description><maintainer email="feiyuwang1998@gmail.com">wangfeiyu</maintainer><license>Proprietary</license><buildtool_depend>ament_python</buildtool_depend><exec_depend>rclpy</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>bt_skill_interfaces</exec_depend><export><build_type>ament_python</build_type></export></package>

ros2/robobrain_services/robobrain_services/monitor_client.py

@@ -0,0 +1,60 @@
+"""Optional advisory loop: active pick/place -> bounded frames -> RoboDopamine.
+No robot commands, completion calls, retry requests or delivery writes exist here.
+"""
+import json
+from robot_robobrain.windows import FrameWindow
+
+def main():
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.action import ActionClient
+    from bt_skill_interfaces.action import EvaluateProgress
+    from bt_skill_interfaces.msg import VisualObservation
+    from std_msgs.msg import String
+    class Monitor(Node):
+        def __init__(self):
+            super().__init__('dense_feedback_client');self.frames=FrameWindow();self.active=None;self.handle=None;self.pending=False;self.sequence=0
+            self.client=ActionClient(self,EvaluateProgress,'monitor/evaluate_progress')
+            self.scene=self.create_subscription(VisualObservation,'observations/scene',self.frame,10)
+            self.registry=self.create_subscription(String,'goal_registry',self.goals,10)
+            self.timer=self.create_timer(1.,self.tick)
+            self.last_registry=0
+        def frame(self,m):self.frames.add(m.header.stamp.sec+m.header.stamp.nanosec/1e9,m.header.frame_id,m.image_path)
+        def goals(self,m):
+            try:
+                data=json.loads(m.data);candidates=[g for g in data['goals'] if g.get('state')==1 and g.get('skill') in ('pick','place') and not g.get('cancel_intent')]
+                candidate=candidates[0] if len(candidates)==1 else None
+                if (candidate or {}).get('goal_id')!=(self.active or {}).get('goal_id'):
+                    if self.handle:self.handle.cancel_goal_async()
+                    self.active=candidate;self.sequence=0
+                self.last_registry=self.get_clock().now().nanoseconds
+            except (ValueError,KeyError,TypeError):self.active=None
+        def tick(self):
+            now=self.get_clock().now().nanoseconds
+            if now<self.last_registry or now-self.last_registry>2_000_000_000:
+                if self.handle:self.handle.cancel_goal_async()
+                self.active=None
+            if self.pending or not self.active or not self.client.server_is_ready():return
+            active=dict(self.active);frames=self.frames.since(active['capture_after_ns']/1e9,now/1e9)
+            if not frames or now/1e9-frames[-1]['stamp']>2:return
+            q=EvaluateProgress.Goal()
+            for key,value in active['trace'].items():setattr(q.trace,key,value)
+            q.task_description=active['skill']+' '+active['target_id'];q.capture_after.sec=active['capture_after_ns']//1_000_000_000;q.capture_after.nanosec=active['capture_after_ns']%1_000_000_000
+            self.sequence+=1;q.sequence=self.sequence;q.window_json=json.dumps(frames);q.timeout.sec=5;self.pending=True
+            def accepted(f):
+                try:
+                    h=f.result()
+                    if not h.accepted:self.pending=False;return
+                    self.handle=h
+                    if (self.active or {}).get('goal_id')!=active['goal_id']:h.cancel_goal_async()
+                    h.get_result_async().add_done_callback(self.finished)
+                except Exception:self.pending=False;self.handle=None
+            try:self.client.send_goal_async(q).add_done_callback(accepted)
+            except Exception:self.pending=False
+        def finished(self,f):self.pending=False;self.handle=None
+    rclpy.init();node=Monitor()
+    try:rclpy.spin(node)
+    finally:
+        if node.handle:node.handle.cancel_goal_async()
+        node.destroy_node();rclpy.shutdown()
+if __name__=='__main__':main()

ros2/robobrain_services/robobrain_services/nodes.py

@@ -0,0 +1,121 @@
+"""ROS adapters; numerical/model logic lives in the separately installable package."""
+import threading,time,json,queue
+from pathlib import Path
+from robot_bt_coordinator.plan import strict_json,canonical
+from robot_robobrain.service import BrainService
+from robot_robobrain.dopamine import DenseFeedbackService
+from robot_robobrain.backends import ProcessBackend,FixtureBackend
+from robot_robobrain.observations import Observation,ObservationCache
+
+def ns(t):return t.sec*1_000_000_000+t.nanosec
+def assign_time(t,n):t.sec=int(n)//1_000_000_000;t.nanosec=int(n)%1_000_000_000
+
+def perception_goal(g,kind):
+    q=dict(task_id=g.task_id,subtask_id=g.subtask_id,target_ref=g.target_ref,
+           target_description=g.target_description,capture_after=ns(g.capture_after),
+           timeout=g.timeout.sec+g.timeout.nanosec/1e9)
+    if kind=='shelf':
+        q.update(source_region_ref=g.source_region_ref,observation_station_id=g.observation_station_id,
+                 station_registry_version=g.station_registry_version)
+    else:
+        q.update(expected_geometry_epoch=g.expected_geometry_epoch,shelf_id=g.shelf_id,
+                 column_id=g.column_id,tier_id=g.tier_id,station_binding_ref=g.station_binding_ref)
+    return q
+
+def run_node(dense=False):
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.action import ActionServer,GoalResponse,CancelResponse
+    from rclpy.callback_groups import ReentrantCallbackGroup
+    from rclpy.executors import MultiThreadedExecutor
+    from bt_skill_interfaces.action import PlanTask,LocateShelfColumn,LocalizeTarget3D,EvaluateProgress
+    from bt_skill_interfaces.msg import VisualObservation,DenseProgress
+    class Server(Node):
+        def __init__(self):
+            super().__init__('robodopamine_server' if dense else 'robobrain_server')
+            self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.busy=False
+            simulation=self.declare_parameter('simulation',False).value
+            record_dir=self.declare_parameter('record_directory','').value
+            if not record_dir:raise ValueError('persistent record_directory required')
+            media=self.declare_parameter('media_root','').value
+            if not media:raise ValueError('trusted media_root required')
+            self.cache=ObservationCache(media)
+            if simulation:
+                if not self.get_namespace().startswith('/sim'):raise ValueError('fixture server restricted to /sim namespace')
+                fixture=self.declare_parameter('fixture_file','').value
+                with open(fixture) as f:raw=strict_json(f.read())
+                backend=FixtureBackend(lambda request:canonical(raw[request['capability']]))
+            else:
+                argv=strict_json(self.declare_parameter('worker_argv_json','[]').value)
+                backend=ProcessBackend(argv,self.declare_parameter('model_version','').value)
+                backend.infer({'capability':'__health__'},float(self.declare_parameter('model_load_timeout_seconds',300.).value))
+            self.service=DenseFeedbackService(backend,record_dir) if dense else BrainService(backend,record_dir)
+            self.publisher=self.create_publisher(DenseProgress,'monitor/dense_progress',10) if dense else None
+            self.subscription=self.create_subscription(VisualObservation,'observations/scene',self.observation,10,callback_group=self.group)
+            self.servers=[]
+            for name,action,kind in ([('monitor/evaluate_progress',EvaluateProgress,'progress')] if dense else [('tasks/plan',PlanTask,'plan'),('skills/locate_shelf_column',LocateShelfColumn,'shelf'),('skills/localize_target_3d',LocalizeTarget3D,'localize3d')]):
+                self.servers.append(ActionServer(self,action,name,execute_callback=lambda h,a=action,k=kind:self.execute(h,a,k),goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group))
+        def observation(self,m):
+            try:self.cache.put(Observation(m.observation_id,ns(m.header.stamp),m.header.frame_id,m.image_path,m.station_id,m.registry_version,m.shelf_id,m.calibration_id,m.geometry_epoch))
+            except (ValueError,OSError) as ex:self.get_logger().warning(str(ex))
+        def admit(self,g):
+            if not 0<g.timeout.sec+g.timeout.nanosec/1e9<=3600 or not 0<=g.timeout.nanosec<1e9:return GoalResponse.REJECT
+            with self.lock:
+                if self.busy:return GoalResponse.REJECT
+                self.busy=True
+            return GoalResponse.ACCEPT
+        def work(self,g,kind,cancel):
+            timeout=g.timeout.sec+g.timeout.nanosec/1e9
+            if kind=='plan':
+                q=dict(task_id=g.task_id,task_revision=g.task_revision,planning_generation=g.planning_generation,instruction=g.instruction,known_info=strict_json(g.known_info_json),context=strict_json(g.context_snapshot_json),constraints=strict_json(g.constraints_json),timeout=timeout)
+                return self.service.plan(q,cancel)
+            if kind=='progress':
+                frames=strict_json(g.window_json)
+                if not isinstance(frames,list):raise ValueError('window must be an array')
+                for frame in frames:
+                    for path in frame['views'].values():
+                        p=Path(path).resolve(strict=True)
+                        if not p.is_relative_to(self.cache.root) or not p.is_file() or p.stat().st_size>32*1024*1024:raise ValueError('invalid window media path')
+                q=dict(task_id=g.trace.task_id,run_id=g.trace.run_id,subtask_id=g.trace.subtask_id,task_description=g.task_description,capture_after=ns(g.capture_after)/1e9,sequence=g.sequence,timeout=timeout)
+                return self.service.evaluate(q,frames,self.get_clock().now().nanoseconds/1e9,cancel)
+            q=perception_goal(g,kind)
+            obs=self.cache.get();method=self.service.shelf if kind=='shelf' else self.service.localize
+            return method(q,obs,self.get_clock().now().nanoseconds,cancel=cancel)
+        def execute(self,h,action,kind):
+            cancel=threading.Event();completed=queue.Queue(maxsize=1);deadline=time.monotonic()+h.request.timeout.sec+h.request.timeout.nanosec/1e9
+            def worker():
+                try:completed.put(self.work(h.request,kind,cancel))
+                except Exception as ex:completed.put(dict(status='FAILED',state='UNKNOWN',error_code='SERVICE_ERROR',message=str(ex)))
+            thread=threading.Thread(target=worker,daemon=True);thread.start();sequence=0;expired=False
+            while thread.is_alive():
+                if h.is_cancel_requested or time.monotonic()>=deadline:cancel.set();expired=time.monotonic()>=deadline
+                sequence+=1;f=action.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=1;f.message='canceling inference' if cancel.is_set() else 'processing';h.publish_feedback(f)
+                thread.join(.2)
+            data=completed.get();r=action.Result()
+            try:
+                if cancel.is_set():data=dict(status='FAILED',state='UNKNOWN',error_code='TIMEOUT' if expired else 'CANCELED',record_ref=data.get('record_ref',''))
+                if kind=='plan':
+                    r.status={'PLAN_READY':0,'NEEDS_CLARIFICATION':1}.get(data.get('status'),2);r.task_plan_json=canonical(data.get('plan',{}));r.planning_record_ref=data.get('record_ref','');r.error_code=data.get('error_code','');r.message=data.get('message','')
+                elif kind=='progress':
+                    f=r.feedback_state;f.trace=h.request.trace;f.sequence=h.request.sequence;f.state=data.get('state','UNKNOWN');f.progress=float(data.get('progress',0));f.progress_valid=f.state!='UNKNOWN';f.hop_json=canonical(data.get('hop'));f.record_ref=data.get('record_ref','');assign_time(f.observed_at,int(data.get('stamp',0)*1e9));r.error_code=data.get('error_code','');self.publisher.publish(f)
+                else:
+                    r.status={'SUCCEEDED':0,'NOT_FOUND':2,'AMBIGUOUS':3}.get(data.get('status'),1);r.error_code=data.get('error_code','');r.message=data.get('message','');r.record_ref=data.get('record_ref','')
+                    if kind=='shelf':
+                        for field in ('shelf_id','side_id','column_id','tier_id','observation_id'):setattr(r,field,data.get(field,''))
+                        r.confidence=float(data.get('confidence',0));assign_time(r.observed_at,data.get('observed_at',0))
+                    else:
+                        r.target_ref=h.request.target_ref;r.geometry_valid=False;r.grasp_point_valid=False;r.position_error_bound_valid=False;r.measurement_source=1;r.quality_code=data.get('quality_code','INVALID');r.observation_id=data.get('observation_id','');r.calibration_id=data.get('calibration_id','');r.geometry_epoch=data.get('geometry_epoch',0)
+                        if 'target_point' in data:
+                            p=data['target_point'];r.target_point.header.frame_id=p['frame_id'];assign_time(r.target_point.header.stamp,p['stamp_ns']);assign_time(r.rgb_stamp,p['stamp_ns']);r.target_point.point.x,r.target_point.point.y,r.target_point.point.z=map(float,p['point'])
+                if h.is_cancel_requested:h.canceled()
+                elif expired:h.abort()
+                elif kind=='plan' or kind=='progress' or data.get('status')=='SUCCEEDED':h.succeed()
+                else:h.abort()
+            finally:
+                with self.lock:self.busy=False
+            return r
+    rclpy.init();node=Server();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node)
+    try:executor.spin()
+    finally:node.service.backend.close();executor.shutdown();node.destroy_node();rclpy.shutdown()
+def brain_main():run_node(False)
+def dopamine_main():run_node(True)

ros2/robobrain_services/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/robobrain_services
+[install]
+install_scripts=$base/lib/robobrain_services

ros2/robobrain_services/setup.py

@@ -0,0 +1,2 @@
+from setuptools import setup
+setup(name='robobrain_services',version='1.2.0',packages=['robobrain_services'],data_files=[('share/ament_index/resource_index/packages',['resource/robobrain_services']),('share/robobrain_services',['package.xml'])],entry_points={'console_scripts':['dense_feedback_client=robobrain_services.monitor_client:main','brain_server=robobrain_services.nodes:brain_main','dopamine_server=robobrain_services.nodes:dopamine_main']})