fix cancel error

unilabos/ros/nodes/presets/host_node.py

@@ -734,46 +734,116 @@ class HostNode(BaseROS2DeviceNode):
     def get_result_callback(self, item: "QueueItem", action_id: str, future) -> None:
         """获取结果回调"""
         job_id = item.job_id
-        result_msg = future.result().result
-        result_data = convert_from_ros_msg(result_msg)
-        status = "success"
-        return_info_str = result_data.get("return_info")
-        if return_info_str is not None:
-            try:
-                return_info = json.loads(return_info_str)
-                suc = return_info.get("suc", False)
-                if not suc:
-                    status = "failed"
-            except json.JSONDecodeError:
+
+        try:
+            result = future.result()
+            result_msg = result.result
+            goal_status = result.status
+
+            # 检查是否是被取消的任务
+            if goal_status == GoalStatus.STATUS_CANCELED:
+                self.lab_logger().info(f"[Host Node] Goal {action_id} ({job_id[:8]}) was cancelled")
                 status = "failed"
-                return_info = serialize_result_info("", False, result_data)
-                self.lab_logger().critical("错误的return_info类型，请断点修复")
-        else:
-            # 无 return_info 字段时，回退到 success 字段（若存在）
-            suc_field = result_data.get("success")
-            if isinstance(suc_field, bool):
-                status = "success" if suc_field else "failed"
-                return_info = serialize_result_info("", suc_field, result_data)
+                return_info = serialize_result_info("Job was cancelled", False, {})
             else:
-                # 最保守的回退：标记失败并返回空JSON
-                status = "failed"
-                return_info = serialize_result_info("缺少return_info", False, result_data)
+                result_data = convert_from_ros_msg(result_msg)
+                status = "success"
+                return_info_str = result_data.get("return_info")
+                if return_info_str is not None:
+                    try:
+                        return_info = json.loads(return_info_str)
+                        suc = return_info.get("suc", False)
+                        if not suc:
+                            status = "failed"
+                    except json.JSONDecodeError:
+                        status = "failed"
+                        return_info = serialize_result_info("", False, result_data)
+                        self.lab_logger().critical("错误的return_info类型，请断点修复")
+                else:
+                    # 无 return_info 字段时，回退到 success 字段（若存在）
+                    suc_field = result_data.get("success")
+                    if isinstance(suc_field, bool):
+                        status = "success" if suc_field else "failed"
+                        return_info = serialize_result_info("", suc_field, result_data)
+                    else:
+                        # 最保守的回退：标记失败并返回空JSON
+                        status = "failed"
+                        return_info = serialize_result_info("缺少return_info", False, result_data)
 
-        self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id}): {status}")
-        self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
+            self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id[:8]}): {status}")
+            if goal_status != GoalStatus.STATUS_CANCELED:
+                self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
 
-        if job_id:
+            # 清理 _goals 中的记录
+            if job_id in self._goals:
+                del self._goals[job_id]
+                self.lab_logger().debug(f"[Host Node] Removed goal {job_id[:8]} from _goals")
+
+            # 发布状态到桥接器
+            if job_id:
+                for bridge in self.bridges:
+                    if hasattr(bridge, "publish_job_status"):
+                        if goal_status == GoalStatus.STATUS_CANCELED:
+                            bridge.publish_job_status({}, item, status, return_info)
+                        else:
+                            bridge.publish_job_status(result_data, item, status, return_info)
+
+        except Exception as e:
+            self.lab_logger().error(
+                f"[Host Node] Error in get_result_callback for {action_id} ({job_id[:8]}): {str(e)}"
+            )
+            import traceback
+
+            self.lab_logger().error(traceback.format_exc())
+
+            # 清理 _goals 中的记录
+            if job_id in self._goals:
+                del self._goals[job_id]
+
+            # 发布失败状态
             for bridge in self.bridges:
                 if hasattr(bridge, "publish_job_status"):
-                    bridge.publish_job_status(result_data, item, status, return_info)
+                    bridge.publish_job_status(
+                        {}, item, "failed", serialize_result_info(f"Callback error: {str(e)}", False, {})
+                    )
 
-    def cancel_goal(self, goal_uuid: str) -> None:
-        """取消目标"""
+    def cancel_goal(self, goal_uuid: str) -> bool:
+        """
+        取消目标
+
+        Args:
+            goal_uuid: 目标UUID（job_id）
+
+        Returns:
+            bool: 如果找到目标并发起取消请求返回True，否则返回False
+        """
         if goal_uuid in self._goals:
-            self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid}")
-            self._goals[goal_uuid].cancel_goal_async()
+            self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid[:8]}")
+            goal_handle = self._goals[goal_uuid]
+
+            # 发起异步取消请求
+            cancel_future = goal_handle.cancel_goal_async()
+
+            # 添加取消完成的回调
+            cancel_future.add_done_callback(lambda future: self._cancel_goal_callback(goal_uuid, future))
+            return True
         else:
-            self.lab_logger().warning(f"[Host Node] Goal {goal_uuid} not found, cannot cancel")
+            self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} not found in _goals, cannot cancel")
+            return False
+
+    def _cancel_goal_callback(self, goal_uuid: str, future) -> None:
+        """取消目标的回调"""
+        try:
+            cancel_response = future.result()
+            if cancel_response.goals_canceling:
+                self.lab_logger().info(f"[Host Node] Goal {goal_uuid[:8]} cancel request accepted")
+            else:
+                self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} cancel request rejected")
+        except Exception as e:
+            self.lab_logger().error(f"[Host Node] Error cancelling goal {goal_uuid[:8]}: {str(e)}")
+            import traceback
+
+            self.lab_logger().error(traceback.format_exc())
 
     def get_goal_status(self, job_id: str) -> int:
         """获取目标状态"""