fix: harden task recovery and DR contract handling

ros2/robobrain_services/package.xml

@@ -1,2 +1,2 @@
-<?xml version="1.2.0"?>
+<?xml version="1.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/nodes.py

@@ -10,6 +10,32 @@ 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 request_snapshot(value):
+    """Keep malformed JSON strings intact when recording a failed ROS request."""
+    if value is None or isinstance(value,(str,int,float,bool)):
+        return value
+    if isinstance(value,dict):
+        return {str(key):request_snapshot(item) for key,item in value.items()}
+    if isinstance(value,(list,tuple)):
+        return [request_snapshot(item) for item in value]
+    fields=getattr(value,'get_fields_and_field_types',None)
+    if callable(fields):
+        return {key:request_snapshot(getattr(value,key)) for key in fields()}
+    if hasattr(value,'__dict__'):
+        return request_snapshot(vars(value))
+    return repr(value)
+
+def guarded_work(service,kind,request,operation):
+    """Always return a terminal failure, including when the archive is unavailable."""
+    try:return operation()
+    except Exception as ex:
+        result=dict(status='FAILED',state='UNKNOWN',error_code='SERVICE_ERROR',message=str(ex))
+        try:result['record_ref']=service._record(kind,request_snapshot(request),'',result)
+        except Exception as archive_error:
+            result['record_ref']=''
+            result['message']+='; diagnostic archive unavailable: '+str(archive_error)
+        return result
+
 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),
@@ -84,8 +110,7 @@ def run_node(dense=False):
         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)))
+                completed.put(guarded_work(self.service,kind,h.request,lambda:self.work(h.request,kind,cancel)))
             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
@@ -105,6 +130,7 @@ def run_node(dense=False):
                         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)
+                        assign_time(r.rgb_stamp,data.get('observed_at',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()