22
22
# Path planning modules from smarc_modelling go here
23
23
from smarc_modelling .vehicles .SAM import SAM
24
24
from smarc_modelling .motion_planning .MotionPrimitives .MainScript import MotionPlanningROS
25
+ #from smarc_modelling.sam_sim import plot_results, Sol
25
26
26
27
class SamPathPlanner (Node ):
27
28
@@ -52,14 +53,18 @@ def __init__(self):
52
53
# Synch subscribers here
53
54
self .lcg_fb = Subscriber (self , PercentStamped , SamTopics .LCG_FB_TOPIC )
54
55
self .vbs_fb = Subscriber (self , PercentStamped , SamTopics .VBS_FB_TOPIC )
55
- self .thrusters_cmd = Subscriber (self , ThrusterAngles , SamTopics .THRUST_VECTOR_CMD_TOPIC )
56
- self .rpm1_fb = Subscriber (self , ThrusterFeedback , SamTopics .THRUSTER1_FB_TOPIC )
57
- self .rpm2_fb = Subscriber (self , ThrusterFeedback , SamTopics .THRUSTER2_FB_TOPIC )
56
+ #self.thrusters_cmd = Subscriber(self, ThrusterAngles , SamTopics.THRUST_VECTOR_CMD_TOPIC)
57
+ self .thrusters_cmd = Subscriber (self , ThrusterAngles , 'with_header/thrust_vector_cmd' )
58
+ # self.rpm1_fb = Subscriber(self, ThrusterFeedback, SamTopics.THRUSTER1_FB_TOPIC)
59
+ # self.rpm2_fb = Subscriber(self, ThrusterFeedback, SamTopics.THRUSTER2_FB_TOPIC)
60
+ self .rpm1_fb = Subscriber (self , ThrusterFeedback , 'with_header/thruster1_fb' )
61
+ self .rpm2_fb = Subscriber (self , ThrusterFeedback , 'with_header/thruster2_fb' )
58
62
59
63
self .ctrl_synch_msg = ApproximateTimeSynchronizer (
60
- [self .vbs_fb , self .lcg_fb , self . thrusters_cmd , self . rpm1_fb , self . rpm2_fb ],
64
+ [self .vbs_fb , self .lcg_fb ],
61
65
queue_size = 100 ,
62
- slop = 0.0001
66
+ slop = 10 ,
67
+ allow_headerless = True
63
68
)
64
69
self .ctrl_synch_msg .registerCallback (self .ctrl_synch_cb )
65
70
@@ -102,8 +107,15 @@ def run(self):
102
107
103
108
rate = self .create_rate (self .node_rate ) # Hz rate
104
109
while rclpy .ok ():
105
- rclpy .spin_once (self , timeout_sec = 0.0 )
106
-
110
+ #rclpy.spin_once(self, timeout_sec=0.0)
111
+
112
+ if self .sam_goal_t == PoseStamped ():
113
+ self ._logger .info (f"Missing goal" )
114
+ if self .sam_pose_t == None :
115
+ self ._logger .info (f"Missing pose" )
116
+ if self .sam_control_t == None :
117
+ self ._logger .info (f"Missing control" )
118
+
107
119
# If goal is empty or feedback has not been received yet, keep spinning
108
120
if self .sam_goal_t != PoseStamped () and self .sam_pose_t != None and self .sam_control_t != None :
109
121
@@ -125,11 +137,35 @@ def run(self):
125
137
self .sam_pose_t .twist .twist .angular .z ,
126
138
self .sam_control_t .vbs .value ,
127
139
self .sam_control_t .lcg .value ,
128
- self .sam_control_t .thruster_angles .thruster_vertical_radians ,
129
- self .sam_control_t .thruster_angles .thruster_horizontal_radians ,
130
- self .sam_control_t .rpms .thruster_1_rpm ,
131
- self .sam_control_t .rpms .thruster_2_rpm
140
+ 0. , 0. , 0. , 0
141
+ # self.sam_control_t.thruster_angles.thruster_vertical_radians,
142
+ # self.sam_control_t.thruster_angles.thruster_horizontal_radians,
143
+ # self.sam_control_t.rpms.thruster_1_rpm,
144
+ # self.sam_control_t.rpms.thruster_2_rpm
132
145
])
146
+
147
+ # # Getting the current orientation of the goal
148
+ # q0_goal_before = self.sam_goal_t.pose.orientation.w
149
+ # q1_goal_before = self.sam_goal_t.pose.orientation.x
150
+ # q2_goal_before = self.sam_goal_t.pose.orientation.y
151
+ # q3_goal_before = self.sam_goal_t.pose.orientation.z
152
+ # r = R.from_quat([q1_goal_before, q2_goal_before, q3_goal_before, q0_goal_before])
153
+ # roll, pitch, yaw = r.as_euler('xyz', degrees=False)
154
+
155
+
156
+ # # From Euler to Quat
157
+ # #yaw = np.deg2rad(20)
158
+ # #pitch = np.deg2rad(-10)
159
+ # roll = np.deg2rad(0)
160
+ # r = R.from_euler('zyx', [yaw, pitch, roll])
161
+ # q = r.as_quat()
162
+ # q0 = q[3]
163
+ # q1, q2, q3 = q[0:3]
164
+ # self.get_logger().info(f"Yaw:...{yaw:.2f}, Pitch:{pitch:.2f}, Roll:{roll}")
165
+
166
+ # r = R.from_quat([q1, q2, q3, q0])
167
+ # roll, pitch, yaw = r.as_euler('xyz', degrees=False)
168
+ # self.get_logger().info(f"Yaw2:...{yaw:.2f}, Pitch2:{pitch:.2f}, Roll2:{roll}")
133
169
134
170
# === End state ===
135
171
end_state = np .array ([
@@ -140,6 +176,7 @@ def run(self):
140
176
self .sam_goal_t .pose .orientation .x ,
141
177
self .sam_goal_t .pose .orientation .y ,
142
178
self .sam_goal_t .pose .orientation .z ,
179
+ # q0,q1,q2,q3,
143
180
0 , 0 , 0 ,
144
181
0 , 0 , 0 ,
145
182
50 , 50 , 0 , 0 , 0 , 0
@@ -150,10 +187,25 @@ def run(self):
150
187
map_boundaries = (self .x_max , self .y_max , self .z_max , self .x_min , self .y_min , self .z_min )
151
188
map_resolution = self .TILESIZE
152
189
190
+ #Print the states
191
+ self .get_logger ().info (f"Initial state:...{ start_state } " )
192
+ self .get_logger ().info (f"-----------" )
193
+ self .get_logger ().info (f"Final State:...{ end_state } " )
194
+
195
+
153
196
## Call the planner
154
197
self .get_logger ().info (f'Calling planner...' )
155
198
trajectory , successful = MotionPlanningROS (start_state , end_state , map_boundaries , map_resolution )
156
199
200
+ # ## Plot the inputs
201
+ # if successful == 1:
202
+ # sol = np.asarray(trajectory).T # the columns are the states
203
+ # t_eval = np.linspace(0, 0.1*len(trajectory), len(trajectory))
204
+ # sol = Sol(t_eval, sol)
205
+
206
+ # plot_results(sol)
207
+ # self.get_logger().info(f'Inputs successfully plotted')
208
+
157
209
## Publish trajectory for Rviz
158
210
self .publishTrajectoryRviz (trajectory )
159
211
@@ -226,11 +278,16 @@ def set_parameters(self):
226
278
# Define your map somewhere here
227
279
228
280
def target_cb (self , msg : PoseStamped ):
229
- self .get_logger ().info (f'---------------Received goal' )
230
- self .sam_goal_t = msg
281
+
282
+ if self .sam_goal_t != msg :
283
+ self .sam_goal_t = msg
284
+ self .get_logger ().info (f'Received new goal' )
285
+
231
286
# The action server will send an empty msg when cancelling
232
287
if self .sam_goal_t == PoseStamped ():
233
288
self .cancel_goal ()
289
+ self .get_logger ().info (f'Cancelled goal' )
290
+
234
291
235
292
def state_cb (self , msg : Odometry ):
236
293
#self.get_logger().info(f'Received state')
@@ -252,14 +309,14 @@ def state_cb(self, msg: Odometry):
252
309
self .sam_pose_t .pose .pose .orientation .y = t .transform .rotation .y
253
310
self .sam_pose_t .pose .pose .orientation .z = t .transform .rotation .z
254
311
255
- def ctrl_synch_cb (self , vbs_fb_msg : PercentStamped , lcg_fb_msg : PercentStamped , dsdr_cmd_msg : ThrusterAngles , rpm1_fb_msg : ThrusterFeedback , rpm2_fb_msg : ThrusterFeedback ):
312
+ def ctrl_synch_cb (self , vbs_fb_msg : PercentStamped , lcg_fb_msg : PercentStamped ):
256
313
#self.get_logger().info(f'Received ctrl inputs')
257
314
self .sam_control_t = SamControl ()
258
315
self .sam_control_t .vbs = vbs_fb_msg
259
316
self .sam_control_t .lcg = lcg_fb_msg
260
- self .sam_control_t .thruster_angles = dsdr_cmd_msg
261
- self .sam_control_t .rpms .thruster_1_rpm = rpm1_fb_msg .rpm .rpm
262
- self .sam_control_t .rpms .thruster_2_rpm = rpm2_fb_msg .rpm .rpm
317
+ # self.sam_control_t.thruster_angles = dsdr_cmd_msg
318
+ # self.sam_control_t.rpms.thruster_1_rpm = rpm1_fb_msg.rpm.rpm
319
+ # self.sam_control_t.rpms.thruster_2_rpm = rpm2_fb_msg.rpm.rpm
263
320
264
321
#### AC for the MPC functions from here
265
322
def send_goal (self , goal_msg ):
0 commit comments