Merge branch 'prcix9320' into feat/lab_resource

unilabos/ros/msgs/message_converter.py

@@ -361,7 +361,14 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any:
         if hasattr(ros_msg, key):
             attr = getattr(ros_msg, key)
             if isinstance(attr, (float, int, str, bool)):
-                setattr(ros_msg, key, type(attr)(value))
+                # 处理list类型的值，取第一个元素或抛出错误
+                if isinstance(value, list):
+                    if len(value) > 0:
+                        setattr(ros_msg, key, type(attr)(value[0]))
+                    else:
+                        setattr(ros_msg, key, type(attr)())  # 使用默认值
+                else:
+                    setattr(ros_msg, key, type(attr)(value))
             elif isinstance(attr, (list, tuple)) and isinstance(value, Iterable):
                 td = ros_msg.SLOT_TYPES[ind].value_type
                 if isinstance(td, NamespacedType):
@@ -374,9 +381,35 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any:
                     setattr(ros_msg, key, [])  # FIXME
             elif "array.array" in str(type(attr)):
                 if attr.typecode == "f" or attr.typecode == "d":
+                    # 如果是单个值，转换为列表
+                    if value is None:
+                        value = []
+                    elif not isinstance(value, Iterable) or isinstance(value, (str, bytes)):
+                        value = [value]
                     setattr(ros_msg, key, [float(i) for i in value])
                 else:
-                    setattr(ros_msg, key, value)
+                    # 对于整数数组，需要确保是序列且每个值在有效范围内
+                    if value is None:
+                        value = []
+                    elif not isinstance(value, Iterable) or isinstance(value, (str, bytes)):
+                        # 如果是单个值，转换为列表
+                        value = [value]
+                    # 确保每个整数值在有效范围内（-2147483648 到 2147483647）
+                    converted_value = []
+                    for i in value:
+                        if i is None:
+                            continue  # 跳过 None 值
+                        if isinstance(i, (int, float)):
+                            int_val = int(i)
+                            # 确保在 int32 范围内
+                            if int_val < -2147483648:
+                                int_val = -2147483648
+                            elif int_val > 2147483647:
+                                int_val = 2147483647
+                            converted_value.append(int_val)
+                        else:
+                            converted_value.append(i)
+                    setattr(ros_msg, key, converted_value)
             else:
                 nested_ros_msg = convert_to_ros_msg(type(attr)(), value)
                 setattr(ros_msg, key, nested_ros_msg)
@@ -770,16 +803,13 @@ def ros_message_to_json_schema(msg_class: Any, field_name: str) -> Dict[str, Any
     return schema
 
 
-def ros_action_to_json_schema(
-    action_class: Any, description="", previous_schema: Optional[Dict[str, Any]] = None
-) -> Dict[str, Any]:
+def ros_action_to_json_schema(action_class: Any, description="") -> Dict[str, Any]:
     """
     将 ROS Action 类转换为 JSON Schema
 
     Args:
         action_class: ROS Action 类
         description: 描述
-        previous_schema: 之前的 schema，用于保留 goal/feedback/result 下一级字段的 description
 
     Returns:
         完整的 JSON Schema 定义
@@ -813,44 +843,9 @@ def ros_action_to_json_schema(
         "required": ["goal"],
     }
 
-    # 保留之前 schema 中 goal/feedback/result 下一级字段的 description
-    if previous_schema:
-        _preserve_field_descriptions(schema, previous_schema)
-
     return schema
 
 
-def _preserve_field_descriptions(
-    new_schema: Dict[str, Any], previous_schema: Dict[str, Any]
-) -> None:
-    """
-    保留之前 schema 中 goal/feedback/result 下一级字段的 description 和 title
-
-    Args:
-        new_schema: 新生成的 schema（会被修改）
-        previous_schema: 之前的 schema
-    """
-    for section in ["goal", "feedback", "result"]:
-        new_section = new_schema.get("properties", {}).get(section, {})
-        prev_section = previous_schema.get("properties", {}).get(section, {})
-
-        if not new_section or not prev_section:
-            continue
-
-        new_props = new_section.get("properties", {})
-        prev_props = prev_section.get("properties", {})
-
-        for field_name, field_schema in new_props.items():
-            if field_name in prev_props:
-                prev_field = prev_props[field_name]
-                # 保留字段的 description
-                if "description" in prev_field and prev_field["description"]:
-                    field_schema["description"] = prev_field["description"]
-                # 保留字段的 title（用户自定义的中文名）
-                if "title" in prev_field and prev_field["title"]:
-                    field_schema["title"] = prev_field["title"]
-
-
 def convert_ros_action_to_jsonschema(
     action_name_or_type: Union[str, Type], output_file: Optional[str] = None, format: str = "json"
 ) -> Dict[str, Any]:

unilabos/ros/nodes/base_device_node.py

@@ -1413,9 +1413,13 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                             if uuid_indices:
                                 uuids = [item[1] for item in uuid_indices]
                                 resource_tree = await self.get_resource(uuids)
-                                plr_resources = resource_tree.to_plr_resources()
+                                plr_resources = resource_tree.to_plr_resources(requested_uuids=uuids)
                                 for i, (idx, _, resource_data) in enumerate(uuid_indices):
-                                    plr_resource = plr_resources[i]
+                                    try:
+                                        plr_resource = plr_resources[i]
+                                    except Exception as e:
+                                        self.lab_logger().error(f"资源查询结果: 共 {len(queried_resources)} 个资源，但查询结果只有 {len(plr_resources)} 个资源，索引为 {i} 的资源不存在")
+                                        raise e
                                     if "sample_id" in resource_data:
                                         plr_resource.unilabos_extra[EXTRA_SAMPLE_UUID] = resource_data["sample_id"]
                                     queried_resources[idx] = plr_resource

unilabos/ros/x/rclpyx.py

@@ -0,0 +1,182 @@
+import asyncio
+from asyncio import events
+import threading
+
+import rclpy
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.executors import await_or_execute, Executor
+from rclpy.action import ActionClient, ActionServer
+from rclpy.action.server import ServerGoalHandle, GoalResponse, GoalInfo, GoalStatus
+from std_msgs.msg import String
+from action_tutorials_interfaces.action import Fibonacci
+
+
+loop = None
+
+def get_event_loop():
+    global loop
+    return loop
+
+
+async def default_handle_accepted_callback_async(goal_handle):
+    """Execute the goal."""
+    await goal_handle.execute()
+
+
+class ServerGoalHandleX(ServerGoalHandle):
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
+    async def execute(self, execute_callback=None):
+        # It's possible that there has been a request to cancel the goal prior to executing.
+        # In this case we want to avoid the illegal state transition to EXECUTING
+        # but still call the users execute callback to let them handle canceling the goal.
+        if not self.is_cancel_requested:
+            self._update_state(_rclpy.GoalEvent.EXECUTE)
+        await self._action_server.notify_execute_async(self, execute_callback)
+
+
+class ActionServerX(ActionServer):
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.register_handle_accepted_callback(default_handle_accepted_callback_async)
+
+    async def _execute_goal_request(self, request_header_and_message):
+        request_header, goal_request = request_header_and_message
+        goal_uuid = goal_request.goal_id
+        goal_info = GoalInfo()
+        goal_info.goal_id = goal_uuid
+
+        self._node.get_logger().debug('New goal request with ID: {0}'.format(goal_uuid.uuid))
+
+        # Check if goal ID is already being tracked by this action server
+        with self._lock:
+            goal_id_exists = self._handle.goal_exists(goal_info)
+
+        accepted = False
+        if not goal_id_exists:
+            # Call user goal callback
+            response = await await_or_execute(self._goal_callback, goal_request.goal)
+            if not isinstance(response, GoalResponse):
+                self._node.get_logger().warning(
+                    'Goal request callback did not return a GoalResponse type. Rejecting goal.')
+            else:
+                accepted = GoalResponse.ACCEPT == response
+
+        if accepted:
+            # Stamp time of acceptance
+            goal_info.stamp = self._node.get_clock().now().to_msg()
+
+            # Create a goal handle
+            try:
+                with self._lock:
+                    goal_handle = ServerGoalHandleX(self, goal_info, goal_request.goal)
+            except RuntimeError as e:
+                self._node.get_logger().error(
+                    'Failed to accept new goal with ID {0}: {1}'.format(goal_uuid.uuid, e))
+                accepted = False
+            else:
+                self._goal_handles[bytes(goal_uuid.uuid)] = goal_handle
+
+        # Send response
+        response_msg = self._action_type.Impl.SendGoalService.Response()
+        response_msg.accepted = accepted
+        response_msg.stamp = goal_info.stamp
+        self._handle.send_goal_response(request_header, response_msg)
+
+        if not accepted:
+            self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid))
+            return
+
+        self._node.get_logger().debug('New goal accepted: {0}'.format(goal_uuid.uuid))
+
+        # Provide the user a reference to the goal handle
+        # await await_or_execute(self._handle_accepted_callback, goal_handle)
+        asyncio.create_task(self._handle_accepted_callback(goal_handle))
+
+    async def notify_execute_async(self, goal_handle, execute_callback):
+        # Use provided callback, defaulting to a previously registered callback
+        if execute_callback is None:
+            if self._execute_callback is None:
+                return
+            execute_callback = self._execute_callback
+
+        # Schedule user callback for execution
+        self._node.get_logger().info(f"{events.get_running_loop()}")
+        asyncio.create_task(self._execute_goal(execute_callback, goal_handle))
+        # loop = asyncio.new_event_loop()
+        # asyncio.set_event_loop(loop)
+        # task = loop.create_task(self._execute_goal(execute_callback, goal_handle))
+        # await task
+
+
+class ActionClientX(ActionClient):
+    feedback_queue = asyncio.Queue()
+
+    async def feedback_cb(self, msg):
+        await self.feedback_queue.put(msg)
+
+    async def send_goal_async(self, goal_msg):
+        goal_future = super().send_goal_async(
+            goal_msg,
+            feedback_callback=self.feedback_cb
+        )
+        client_goal_handle = await asyncio.ensure_future(goal_future)
+        if not client_goal_handle.accepted:
+            raise Exception("Goal rejected.")
+        result_future = client_goal_handle.get_result_async()
+        while True:
+            feedback_future = asyncio.ensure_future(self.feedback_queue.get())
+            tasks = [result_future, feedback_future]
+            await asyncio.wait(tasks, return_when=asyncio.FIRST_COMPLETED)
+            if result_future.done():
+                result = result_future.result().result
+                yield (None, result)
+                break
+            else:
+                feedback = feedback_future.result().feedback
+                yield (feedback, None)
+
+
+async def main(node):
+    print('Node started.')
+    action_client = ActionClientX(node, Fibonacci, 'fibonacci')
+    goal_msg = Fibonacci.Goal()
+    goal_msg.order = 10
+    async for (feedback, result) in action_client.send_goal_async(goal_msg):
+        if feedback:
+            print(f'Feedback: {feedback}')
+        else:
+            print(f'Result: {result}')
+    print('Finished.')
+
+
+async def ros_loop_node(node):
+    while rclpy.ok():
+        rclpy.spin_once(node, timeout_sec=0)
+        await asyncio.sleep(1e-4)
+
+
+async def ros_loop(executor: Executor):
+    while rclpy.ok():
+        executor.spin_once(timeout_sec=0)
+        await asyncio.sleep(1e-4)
+
+
+def run_event_loop():
+    global loop
+    loop = asyncio.new_event_loop()
+    asyncio.set_event_loop(loop)
+    loop.run_forever()
+    
+
+def run_event_loop_in_thread():
+    thread = threading.Thread(target=run_event_loop, args=())
+    thread.start()
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = rclpy.create_node('async_subscriber')
+    future = asyncio.wait([ros_loop(node), main()])
+    asyncio.get_event_loop().run_until_complete(future)