1
1
import argparse
2
+ import asyncio
2
3
import io
3
4
import json
4
5
import os
5
6
import time
6
7
import uuid
7
8
from typing import Any , Dict , List , Optional , Union
8
9
9
- from fastmcp import FastMCP
10
+ from fastmcp import Context , FastMCP
10
11
from fastmcp .utilities .types import Image
11
12
from PIL import Image as PILImage
12
13
@@ -2463,7 +2464,7 @@ def inspect_all_actions() -> dict:
2463
2464
"send_action_goal('/turtle1/rotate_absolute', 'turtlesim/action/RotateAbsolute', {'theta': 1.57})"
2464
2465
)
2465
2466
)
2466
- def send_action_goal (action_name : str , action_type : str , goal : dict , timeout : float = None ) -> dict :
2467
+ async def send_action_goal (action_name : str , action_type : str , goal : dict , timeout : float = None , ctx : Context = None ) -> dict :
2467
2468
"""
2468
2469
Send a goal to a ROS action server. Works only with ROS 2.
2469
2470
@@ -2515,16 +2516,27 @@ def send_action_goal(action_name: str, action_type: str, goal: dict, timeout: fl
2515
2516
actual_timeout = timeout if timeout is not None else 10.0 # Default 10 seconds
2516
2517
start_time = time .time ()
2517
2518
last_feedback = None # Store the last feedback message
2519
+ feedback_count = 0 # Count feedback messages received
2518
2520
2519
2521
while time .time () - start_time < actual_timeout :
2520
- response = ws_manager .receive (timeout = actual_timeout - (time .time () - start_time ))
2522
+ elapsed_time = time .time () - start_time
2523
+
2524
+ response = ws_manager .receive (timeout = actual_timeout - elapsed_time )
2521
2525
2522
2526
if response :
2523
2527
try :
2524
2528
msg_data = json .loads (response )
2525
2529
2526
2530
# Handle action_result messages (final completion)
2527
2531
if msg_data .get ("op" ) == "action_result" :
2532
+ # Report completion
2533
+ if ctx :
2534
+ try :
2535
+ completion_msg = f"Action completed successfully (received { feedback_count } feedback messages)"
2536
+ await ctx .report_progress (progress = feedback_count , total = None , message = completion_msg )
2537
+ except Exception :
2538
+ pass
2539
+
2528
2540
return {
2529
2541
"action" : action_name ,
2530
2542
"action_type" : action_type ,
@@ -2534,16 +2546,35 @@ def send_action_goal(action_name: str, action_type: str, goal: dict, timeout: fl
2534
2546
"result" : msg_data .get ("values" , {}),
2535
2547
}
2536
2548
2537
- # Store action_feedback messages for timeout case
2549
+ # Store action_feedback messages and report progress
2538
2550
if msg_data .get ("op" ) == "action_feedback" :
2551
+ feedback_count += 1
2539
2552
last_feedback = msg_data
2553
+
2554
+ # Report feedback progress
2555
+ if ctx :
2556
+ try :
2557
+ feedback_values = msg_data .get ("values" , {})
2558
+ feedback_msg = f"Action feedback #{ feedback_count } : { str (feedback_values )[:100 ]} ..."
2559
+ await ctx .report_progress (progress = feedback_count , total = None , message = feedback_msg )
2560
+ except Exception :
2561
+ pass
2540
2562
2541
2563
except json .JSONDecodeError :
2542
2564
continue
2565
+ else :
2566
+ # No response received, continue waiting
2567
+ pass
2543
2568
2544
- time .sleep (0.1 )
2569
+ await asyncio .sleep (0.1 )
2545
2570
2546
2571
# Timeout - return last feedback if available
2572
+ if ctx and feedback_count > 0 :
2573
+ try :
2574
+ await ctx .report_progress (progress = feedback_count , total = None , message = f"Action timed out after { actual_timeout } seconds (received { feedback_count } feedback messages)" )
2575
+ except Exception :
2576
+ pass
2577
+
2547
2578
result = {
2548
2579
"action" : action_name ,
2549
2580
"action_type" : action_type ,
0 commit comments