1+ #!/usr/bin/env python3
2+
3+ # Run and send “goto Goal2” once:
4+ # ros2 run amr_core unified_action_client --ros-args -p command_type:=goto_goal -p goal_name:=Goal2 -p exit_on_result:=true
5+ #
6+ # Subscribe to goal poses and send gotoPoint on messages:
7+ # ros2 run amr_core unified_action_client --ros-args -p enable_goal_pose_sub:=true -p goal_pose_topic:=goal_pose
8+ #
9+ # Subscribe to initial poses and send localizeAtPoint on messages:
10+ # ros2 run amr_core unified_action_client --ros-args -p enable_initialpose_sub:=true -p initialpose_topic:=initialpose
11+ #
12+ # Execute a macro or dock:
13+ # ros2 run amr_core unified_action_client --ros-args -p command_type:=execute_macro -p macro_name:=Macro4
14+ # ros2 run amr_core unified_action_client --ros-args -p command_type:=dock -p exit_on_result:=true
15+
16+
17+ import math
18+ import rclpy
19+ from rclpy .node import Node
20+ from rclpy .action import ActionClient
21+ from amr_msgs .action import Action
22+ from geometry_msgs .msg import PoseStamped , PoseWithCovarianceStamped
23+
24+ GOTO_POINT_COMMAND = "doTask gotoPoint "
25+ LOCALIZE_TO_POINT_COMMAND = "localizetopoint "
26+ DEFAULT_GOAL_POSE_TOPIC = "goal_pose"
27+ DEFAULT_INITIAL_POSE_TOPIC = "initialpose"
28+
29+
30+ class UnifiedAmrActionClient (Node ):
31+ def __init__ (self ):
32+ super ().__init__ ("amr_unified_action_client" )
33+ # Action client
34+ self ._ac = ActionClient (self , Action , "action_server" )
35+
36+ # Parameters
37+ self .declare_parameter ("enable_goal_pose_sub" , False )
38+ self .declare_parameter ("enable_initialpose_sub" , False )
39+ self .declare_parameter ("goal_pose_topic" , DEFAULT_GOAL_POSE_TOPIC )
40+ self .declare_parameter ("initialpose_topic" , DEFAULT_INITIAL_POSE_TOPIC )
41+ self .declare_parameter ("command_type" , "none" ) # none|goto_goal|execute_macro|dock
42+ self .declare_parameter ("goal_name" , "Goal1" )
43+ self .declare_parameter ("macro_name" , "Macro1" )
44+ self .declare_parameter ("exit_on_result" , False )
45+
46+ # Subscriptions (optional)
47+ if self .get_parameter ("enable_goal_pose_sub" ).get_parameter_value ().bool_value :
48+ topic = self .get_parameter ("goal_pose_topic" ).get_parameter_value ().string_value
49+ self .create_subscription (PoseStamped , topic , self ._on_goal_pose , 10 )
50+ self .get_logger ().info (f"Subscribed to goal pose: { topic } )
51+
52+ if self .get_parameter ("enable_initialpose_sub" ).get_parameter_value ().bool_value :
53+ topic = self .get_parameter ("initialpose_topic" ).get_parameter_value ().string_value
54+ self .create_subscription (PoseWithCovarianceStamped , topic , self ._on_initial_pose , 10 )
55+ self .get_logger ().info (f"Subscribed to initial pose: { topic } )
56+
57+ # Fire a one-shot command from params if requested
58+ cmd = self .get_parameter ("command_type" ).get_parameter_value ().string_value
59+ if cmd == "goto_goal" :
60+ name = self .get_parameter ("goal_name" ).get_parameter_value ().string_value
61+ self .goto_goal (name )
62+ elif cmd == "execute_macro" :
63+ name = self .get_parameter ("macro_name" ).get_parameter_value ().string_value
64+ self .execute_macro (name )
65+ elif cmd == "dock" :
66+ self .dock ()
67+
68+ # ========== High-level commands ==========
69+ def goto_goal (self , name : str ):
70+ goal = Action .Goal ()
71+ goal .command = "goto " + name
72+ goal .identifier = [f"Arrived at { name } ]
73+ self ._send_goal (goal )
74+
75+ def execute_macro (self , name : str ):
76+ goal = Action .Goal ()
77+ goal .command = "executeMacro " + name
78+ goal .identifier = [f"Completed macro { name } ]
79+ self ._send_goal (goal )
80+
81+ def dock (self ):
82+ goal = Action .Goal ()
83+ goal .command = "dock"
84+ goal .identifier = ["Arrived at dock" ]
85+ self ._send_goal (goal )
86+
87+ # Pose-based commands (from subscriptions)
88+ def _on_goal_pose (self , msg : PoseStamped ):
89+ x = int (msg .pose .position .x * 1000.0 )
90+ y = int (msg .pose .position .y * 1000.0 )
91+ deg = self ._yaw_deg (msg .pose .orientation .w , msg .pose .orientation .x ,
92+ msg .pose .orientation .y , msg .pose .orientation .z )
93+ if deg > 180 :
94+ deg -= 360
95+ coords = f"{ x } { y } { int (deg )}
96+ goal = Action .Goal ()
97+ goal .command = GOTO_POINT_COMMAND + coords
98+ goal .identifier = ["Going to point" ]
99+ self ._send_goal (goal )
100+
101+ def _on_initial_pose (self , msg : PoseWithCovarianceStamped ):
102+ p = msg .pose .pose .position
103+ q = msg .pose .pose .orientation
104+ x = int (p .x * 1000.0 )
105+ y = int (p .y * 1000.0 )
106+ deg = self ._yaw_deg (q .w , q .x , q .y , q .z )
107+ if deg > 180 :
108+ deg -= 360
109+ coords = f"{ x } { y } { int (deg )} # xySpread=0 angleSpread=0
110+ goal = Action .Goal ()
111+ goal .command = LOCALIZE_TO_POINT_COMMAND + coords
112+ goal .identifier = ["Localizing at point" ]
113+ self ._send_goal (goal )
114+
115+ # ========== Action helpers ==========
116+ def _send_goal (self , goal : Action .Goal ):
117+ self ._ac .wait_for_server ()
118+ fut = self ._ac .send_goal_async (goal , feedback_callback = self ._on_feedback )
119+ fut .add_done_callback (self ._on_goal_response )
120+
121+ def _on_goal_response (self , future ):
122+ gh = future .result ()
123+ if not gh .accepted :
124+ self .get_logger ().info ("Goal Rejected!" )
125+ return
126+ self .get_logger ().info (f"Goal: { gh .goal_request .goal .command } )
127+ res_fut = gh .get_result_async ()
128+ res_fut .add_done_callback (self ._on_result )
129+
130+ def _on_result (self , future ):
131+ result = future .result ().result
132+ self .get_logger ().info (result .res_msg )
133+ if self .get_parameter ("exit_on_result" ).get_parameter_value ().bool_value :
134+ rclpy .shutdown ()
135+
136+ def _on_feedback (self , fb_msg ):
137+ self .get_logger ().info (fb_msg .feedback .feed_msg )
138+
139+ # ========== Math ==========
140+ def _yaw_deg (self , rw , rx , ry , rz ) -> float :
141+ t3 = + 2.0 * (rw * rz + rx * ry )
142+ t4 = + 1.0 - 2.0 * (ry * ry + rz * rz )
143+ yaw = round (math .atan2 (t3 , t4 ), 5 ) # radians
144+ return yaw * 57.296 # degrees (approx 180/pi)
145+
146+
147+ def main (args = None ):
148+ rclpy .init (args = args )
149+ node = UnifiedAmrActionClient ()
150+ rclpy .spin (node )
151+ rclpy .shutdown ()
152+
153+
154+ if __name__ == "__main__" :
155+ main ()
0 commit comments