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,9 @@ 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 (
2468
+ action_name : str , action_type : str , goal : dict , timeout : float = None , ctx : Context = None
2469
+ ) -> dict :
2467
2470
"""
2468
2471
Send a goal to a ROS action server. Works only with ROS 2.
2469
2472
@@ -2515,16 +2518,29 @@ def send_action_goal(action_name: str, action_type: str, goal: dict, timeout: fl
2515
2518
actual_timeout = timeout if timeout is not None else 10.0 # Default 10 seconds
2516
2519
start_time = time .time ()
2517
2520
last_feedback = None # Store the last feedback message
2521
+ feedback_count = 0 # Count feedback messages received
2518
2522
2519
2523
while time .time () - start_time < actual_timeout :
2520
- response = ws_manager .receive (timeout = actual_timeout - (time .time () - start_time ))
2524
+ elapsed_time = time .time () - start_time
2525
+
2526
+ response = ws_manager .receive (timeout = actual_timeout - elapsed_time )
2521
2527
2522
2528
if response :
2523
2529
try :
2524
2530
msg_data = json .loads (response )
2525
2531
2526
2532
# Handle action_result messages (final completion)
2527
2533
if msg_data .get ("op" ) == "action_result" :
2534
+ # Report completion
2535
+ if ctx :
2536
+ try :
2537
+ completion_msg = f"Action completed successfully (received { feedback_count } feedback messages)"
2538
+ await ctx .report_progress (
2539
+ progress = feedback_count , total = None , message = completion_msg
2540
+ )
2541
+ except Exception :
2542
+ pass
2543
+
2528
2544
return {
2529
2545
"action" : action_name ,
2530
2546
"action_type" : action_type ,
@@ -2534,16 +2550,41 @@ def send_action_goal(action_name: str, action_type: str, goal: dict, timeout: fl
2534
2550
"result" : msg_data .get ("values" , {}),
2535
2551
}
2536
2552
2537
- # Store action_feedback messages for timeout case
2553
+ # Store action_feedback messages and report progress
2538
2554
if msg_data .get ("op" ) == "action_feedback" :
2555
+ feedback_count += 1
2539
2556
last_feedback = msg_data
2540
2557
2558
+ # Report feedback progress
2559
+ if ctx :
2560
+ try :
2561
+ feedback_values = msg_data .get ("values" , {})
2562
+ feedback_msg = f"Action feedback #{ feedback_count } : { str (feedback_values )[:100 ]} ..."
2563
+ await ctx .report_progress (
2564
+ progress = feedback_count , total = None , message = feedback_msg
2565
+ )
2566
+ except Exception :
2567
+ pass
2568
+
2541
2569
except json .JSONDecodeError :
2542
2570
continue
2571
+ else :
2572
+ # No response received, continue waiting
2573
+ pass
2543
2574
2544
- time .sleep (0.1 )
2575
+ await asyncio .sleep (0.1 )
2545
2576
2546
2577
# Timeout - return last feedback if available
2578
+ if ctx and feedback_count > 0 :
2579
+ try :
2580
+ await ctx .report_progress (
2581
+ progress = feedback_count ,
2582
+ total = None ,
2583
+ message = f"Action timed out after { actual_timeout } seconds (received { feedback_count } feedback messages)" ,
2584
+ )
2585
+ except Exception :
2586
+ pass
2587
+
2547
2588
result = {
2548
2589
"action" : action_name ,
2549
2590
"action_type" : action_type ,
0 commit comments