Skip to content

Commit 524fc75

Browse files
authored
Merge pull request #180 from ignaciotb/humble
Latest path planner for Sam in pool PC
2 parents b57be91 + 2b5fd93 commit 524fc75

File tree

5 files changed

+159
-23
lines changed

5 files changed

+159
-23
lines changed

behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_server.py

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -237,13 +237,21 @@ def execution_callback(self, goal_handle: ServerGoalHandle) -> ActionResult:
237237
result_msg = self.action_type.Result
238238
hydropoint = self._json_ops.decode(goal_handle.request.goal, ActC.GOAL)
239239
self.logger.info(f"Hydropoint sent: {hydropoint}")
240+
# rate = self._node.create_rate()
241+
242+
# rate.sleep()
243+
#self.feedback_loop(hydropoint, goal_handle)
244+
245+
# Action succeeded
246+
#goal_handle.succeed()
240247
time.sleep(5)
241248
self._pub_setpoint.publish(hydropoint)
242249
status = self.feedback_loop(hydropoint, goal_handle)
243250
if status == "cancelled":
244251
self.logger.info("Goal was cancelled by client.")
245252
result_msg.success = False
246253
return result_msg
254+
247255
result_msg.success = True
248256
return result_msg
249257

@@ -314,11 +322,14 @@ def feedback_loop(self, pose_stamped: PoseStamped, goal_handle: ServerGoalHandle
314322
feedback = self.action_type.Feedback
315323
tol_check = self._tol_check(d)
316324
while not tol_check:
325+
self._pub_setpoint.publish(pose_stamped)
326+
317327
if goal_handle.is_cancel_requested:
318328
self.logger.info("Goal was cancelled by client.")
319329
goal_handle.canceled()
320330
self.publish_stop_setpoint()
321331
return "cancelled"
332+
322333
feedback.feedback = self._json_ops.encode(d)
323334
goal_handle.publish_feedback(feedback)
324335
rate.sleep()

path_planning/sam_path_planning/launch/sam_path_planning.launch

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
<launch>
22
<arg name="robot_name" default="sam" />
33

4-
<node pkg="sam_path_planning" exec="sam_planner_node" name="sam_planner_node" output="screen" namespace="$(var robot_name)">
5-
<param name="robot_name" value="$(var robot_name)" />
6-
<param name="use_sim_time" value="false" />
4+
<node pkg="sam_path_planning" exec="topic_collector_node" name="topic_collector_node" output="screen"
5+
namespace="$(var robot_name)">
76
</node>
87

9-
<node pkg="go_to_hydrobaticpoint" exec="client" name="hydrobatic_client" output="screen" namespace="$(var robot_name)">
8+
<node pkg="sam_path_planning" exec="sam_planner_node" name="sam_planner_node" output="screen"
9+
namespace="$(var robot_name)">
1010
<param name="robot_name" value="$(var robot_name)" />
1111
<param name="use_sim_time" value="false" />
1212
</node>
@@ -22,11 +22,16 @@
2222
<param name="goal_threshold" value="9" />
2323
</node>
2424

25+
<node pkg="go_to_hydrobaticpoint" exec="client" name="hydrobatic_client" output="screen" namespace="$(var robot_name)">
26+
<param name="robot_name" value="$(var robot_name)" />
27+
<param name="use_sim_time" value="false" />
28+
</node>
29+
2530
<node pkg="sam_emergency_action" exec="server" name="emergency_server" output="screen" namespace="$(var robot_name)">
2631
<param name="use_sim_time" value="false" />
2732
<param name="robot_name" value="$(var robot_name)"/>
2833
<param name="pub_frequency" value="10"/>
2934
<param name="lcg_percentage" value="40.0"/>
3035
</node>
3136

32-
</launch>
37+
</launch>

path_planning/sam_path_planning/sam_path_planning/sam_planner_node.py

Lines changed: 74 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
# Path planning modules from smarc_modelling go here
2323
from smarc_modelling.vehicles.SAM import SAM
2424
from smarc_modelling.motion_planning.MotionPrimitives.MainScript import MotionPlanningROS
25+
#from smarc_modelling.sam_sim import plot_results, Sol
2526

2627
class SamPathPlanner(Node):
2728

@@ -52,14 +53,18 @@ def __init__(self):
5253
# Synch subscribers here
5354
self.lcg_fb = Subscriber(self, PercentStamped, SamTopics.LCG_FB_TOPIC)
5455
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')
5862

5963
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],
6165
queue_size = 100,
62-
slop = 0.0001
66+
slop = 10,
67+
allow_headerless=True
6368
)
6469
self.ctrl_synch_msg.registerCallback(self.ctrl_synch_cb)
6570

@@ -102,8 +107,15 @@ def run(self):
102107

103108
rate = self.create_rate(self.node_rate) # Hz rate
104109
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+
107119
# If goal is empty or feedback has not been received yet, keep spinning
108120
if self.sam_goal_t != PoseStamped() and self.sam_pose_t != None and self.sam_control_t != None:
109121

@@ -125,11 +137,35 @@ def run(self):
125137
self.sam_pose_t.twist.twist.angular.z,
126138
self.sam_control_t.vbs.value,
127139
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
132145
])
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}")
133169

134170
# === End state ===
135171
end_state = np.array([
@@ -140,6 +176,7 @@ def run(self):
140176
self.sam_goal_t.pose.orientation.x,
141177
self.sam_goal_t.pose.orientation.y,
142178
self.sam_goal_t.pose.orientation.z,
179+
# q0,q1,q2,q3,
143180
0, 0, 0,
144181
0, 0, 0,
145182
50, 50, 0, 0, 0, 0
@@ -150,10 +187,25 @@ def run(self):
150187
map_boundaries = (self.x_max, self.y_max, self.z_max, self.x_min, self.y_min, self.z_min)
151188
map_resolution = self.TILESIZE
152189

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+
153196
## Call the planner
154197
self.get_logger().info(f'Calling planner...')
155198
trajectory, successful = MotionPlanningROS(start_state, end_state, map_boundaries, map_resolution)
156199

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+
157209
## Publish trajectory for Rviz
158210
self.publishTrajectoryRviz(trajectory)
159211

@@ -226,11 +278,16 @@ def set_parameters(self):
226278
# Define your map somewhere here
227279

228280
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+
231286
# The action server will send an empty msg when cancelling
232287
if self.sam_goal_t == PoseStamped():
233288
self.cancel_goal()
289+
self.get_logger().info(f'Cancelled goal')
290+
234291

235292
def state_cb(self, msg: Odometry):
236293
#self.get_logger().info(f'Received state')
@@ -252,14 +309,14 @@ def state_cb(self, msg: Odometry):
252309
self.sam_pose_t.pose.pose.orientation.y = t.transform.rotation.y
253310
self.sam_pose_t.pose.pose.orientation.z = t.transform.rotation.z
254311

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):
256313
#self.get_logger().info(f'Received ctrl inputs')
257314
self.sam_control_t = SamControl()
258315
self.sam_control_t.vbs = vbs_fb_msg
259316
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
263320

264321
#### AC for the MPC functions from here
265322
def send_goal(self, goal_msg):
Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
#!/usr/bin/env python3
2+
# -*- coding: utf-8 -*-
3+
4+
import rclpy
5+
from rclpy.node import Node
6+
from rclpy.executors import MultiThreadedExecutor
7+
from smarc_msgs.msg import ThrusterFeedback
8+
from sam_msgs.msg import ThrusterAngles
9+
10+
# Some of the messages are missing timestamp so we make new ones
11+
class AddTimestamp(Node):
12+
def __init__(self):
13+
super().__init__('add_timestamps')
14+
15+
# NOTE: Since we reassign the stamps for some of the topics we need to do it to all topics we are going to use
16+
# so that they exist in the same "timeline"
17+
# Creating publishers
18+
self.thruster1_fb_pub = self.create_publisher(ThrusterFeedback, "with_header/thruster1_fb", 10)
19+
self.thruster2_fb_pub = self.create_publisher(ThrusterFeedback, "with_header/thruster2_fb", 10)
20+
self.thruster_vector_pub = self.create_publisher(ThrusterAngles, "with_header/thrust_vector_cmd", 10)
21+
22+
# Subscribe to topics we want to add timestamps to
23+
self.thruster1_fb_sub = self.create_subscription(ThrusterFeedback, "/sam/core/thruster1_fb", self.add_stamp_thruster1fb, 10) # No data in stamp
24+
self.thruster2_fb_sub = self.create_subscription(ThrusterFeedback, "/sam/core/thruster2_fb", self.add_stamp_thruster2fb, 10) # No data in stamp
25+
self.thrusters_cmd_sub = self.create_subscription(ThrusterAngles, "/sam/core/thrust_vector_cmd", self.add_stamp_vector, 10) # No data in stamp
26+
27+
def add_stamp_vector(self,msg):
28+
29+
msg_stamped = msg
30+
msg_stamped.header.stamp = self.get_clock().now().to_msg()
31+
self.thruster_vector_pub.publish(msg_stamped)
32+
33+
def add_stamp_thruster2fb(self, msg):
34+
msg_stamped = msg
35+
msg_stamped.header.stamp = self.get_clock().now().to_msg()
36+
self.thruster2_fb_pub.publish(msg_stamped)
37+
38+
def add_stamp_thruster1fb(self, msg):
39+
40+
msg_stamped = msg
41+
msg_stamped.header.stamp = self.get_clock().now().to_msg()
42+
43+
self.thruster1_fb_pub.publish(msg_stamped)
44+
45+
46+
def main(args=None):
47+
# Start and run node
48+
rclpy.init(args=args)
49+
50+
node_stamp = AddTimestamp()
51+
52+
executor = MultiThreadedExecutor()
53+
executor.add_node(node_stamp)
54+
55+
56+
executor.spin()
57+
58+
node_stamp.destroy_node()
59+
rclpy.shutdown()
60+
61+
if __name__ == '__main__':
62+
main()

path_planning/sam_path_planning/setup.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,8 @@
2424
tests_require=['pytest'],
2525
entry_points={
2626
'console_scripts': [
27-
'sam_planner_node = sam_path_planning.sam_planner_node:main'
27+
'sam_planner_node = sam_path_planning.sam_planner_node:main',
28+
'topic_collector_node = sam_path_planning.topic_collector:main'
2829
],
2930
},
3031
)

0 commit comments

Comments
 (0)