From 3bc7bb0bef218e92657ff8d645d5a47e8ea5af3f Mon Sep 17 00:00:00 2001 From: pool man Date: Tue, 8 Jul 2025 16:45:16 +0200 Subject: [PATCH 1/3] Crazy little bugs --- .../go_to_hydrobaticpoint/hydrobaticpoint_action.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_action.py b/behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_action.py index cb9e88c1..ff7bcad8 100644 --- a/behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_action.py +++ b/behaviours/go_to_hydrobaticpoint/go_to_hydrobaticpoint/hydrobaticpoint_action.py @@ -35,8 +35,8 @@ def decode( hydropoint = PoseStamped() hydropoint.header.frame_id = str(fmt_dict["hydropoint"]["frame_id"]) hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["x"]) - hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["y"]) - hydropoint.pose.position.x = float(fmt_dict["hydropoint"]["position"]["z"]) + hydropoint.pose.position.y = float(fmt_dict["hydropoint"]["position"]["y"]) + hydropoint.pose.position.z = float(fmt_dict["hydropoint"]["position"]["z"]) hydropoint.pose.orientation.x = float(fmt_dict["hydropoint"]["orientation"]["x"]) hydropoint.pose.orientation.y = float(fmt_dict["hydropoint"]["orientation"]["y"]) hydropoint.pose.orientation.z = float(fmt_dict["hydropoint"]["orientation"]["z"]) From 4b9fce4288376cb9807528776832c21af3a4ec85 Mon Sep 17 00:00:00 2001 From: ignaciotb Date: Tue, 15 Jul 2025 10:17:57 +0200 Subject: [PATCH 2/3] Latests assisted manual diving ran on Sam in the pool --- .../config/sam_diving_controller_config.yaml | 2 +- .../sam_diving_controller/JoyNode.py | 10 +- .../launch/joy_depth_control.launch | 2 +- .../sam_joy_xbox/press_to_depth.py | 154 ++++++++++++++++++ .../sam_joy_controllers/sam_joy_xbox/setup.py | 1 + .../sam_joy_teleop/test_thrust_vec.py | 70 ++++++++ .../sam_joy_teleop/setup.py | 1 + 7 files changed, 234 insertions(+), 6 deletions(-) create mode 100644 external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/sam_joy_xbox/press_to_depth.py create mode 100755 external_equipment/sam_joy_controllers/sam_joy_teleop/sam_joy_teleop/test_thrust_vec.py diff --git a/behaviours/sam_diving_controller/config/sam_diving_controller_config.yaml b/behaviours/sam_diving_controller/config/sam_diving_controller_config.yaml index 42ef5dd2..d999de55 100644 --- a/behaviours/sam_diving_controller/config/sam_diving_controller_config.yaml +++ b/behaviours/sam_diving_controller/config/sam_diving_controller_config.yaml @@ -1,7 +1,7 @@ --- /*/diving_node: ros__parameters: - view_rate: 0.1 + view_rate: 0.2 model_rate: 0.1 controller_rate: 0.1 vbs_pid_kp: 20.0 diff --git a/behaviours/sam_diving_controller/sam_diving_controller/JoyNode.py b/behaviours/sam_diving_controller/sam_diving_controller/JoyNode.py index f4389662..41b13bcc 100644 --- a/behaviours/sam_diving_controller/sam_diving_controller/JoyNode.py +++ b/behaviours/sam_diving_controller/sam_diving_controller/JoyNode.py @@ -8,6 +8,7 @@ from .DiveSub import DiveSub from .DiveController import DepthJoyControllerPID from .ConveniencePub import ConveniencePub +from .ParamUtils import DivingModelParam from rclpy.executors import MultiThreadedExecutor @@ -31,10 +32,11 @@ def joy_depth(): dive_sub_rate = node.get_parameter('dive_sub_rate').get_parameter_value().double_value convenience_pub_rate = node.get_parameter('convenience_rate').get_parameter_value().double_value - - dive_pub = SAMDivePub(node) - dive_sub = DiveSub(node, dive_pub) - dive_controller = DepthJoyControllerPID(node, dive_pub, dive_sub, dive_controller_rate) + + param = DivingModelParam(node).get_param() + dive_sub = DiveSub(node, param) + dive_pub = SAMDivePub(node, dive_sub, param) + dive_controller = DepthJoyControllerPID(node, dive_pub, dive_sub,param, dive_controller_rate) convenience_pub = ConveniencePub(node, dive_sub, dive_controller) diff --git a/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/launch/joy_depth_control.launch b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/launch/joy_depth_control.launch index 0a55240b..3d467326 100644 --- a/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/launch/joy_depth_control.launch +++ b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/launch/joy_depth_control.launch @@ -7,7 +7,7 @@ - + diff --git a/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/sam_joy_xbox/press_to_depth.py b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/sam_joy_xbox/press_to_depth.py new file mode 100644 index 00000000..5578d55b --- /dev/null +++ b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/sam_joy_xbox/press_to_depth.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import FluidPressure +from geometry_msgs.msg import PoseWithCovarianceStamped + +# Topics +from sam_msgs.msg import Topics as SamTopics +from dead_reckoning_msgs.msg import Topics as DRTopics + +# Frames/Links +from sam_msgs.msg import Links as SamLinks + +try: + from .helpers.ros_helpers import rcl_time_to_stamp +except ImportError: + from helpers.ros_helpers import rcl_time_to_stamp + + +class Press2Depth(Node): + + def __init__(self): + super().__init__("press_to_depth_node") + self.get_logger().info("Starting node defined in press_to_depth.py") + + # ===== Declare parameters ===== + self.declare_node_parameters() + + self.robot_name = self.get_parameter("robot_name").value + + self.odom_frame = self.get_parameter('odom_frame').value + + self.base_frame = f"{self.robot_name}_{SamLinks.BASE_LINK}" + self.press_frame = f"{self.robot_name}_{SamLinks.PRESS_LINK}" # Unused + # Removed depth frame + + self.subs = self.create_subscription(msg_type=FluidPressure, topic="core/vbs_tank_pressure", + callback=self.depthCB, qos_profile=10) + + self.pub = self.create_publisher(msg_type=PoseWithCovarianceStamped, topic=DRTopics.DR_DEPTH_POSE_TOPIC, + qos_profile=10) + + self.depth_msg = PoseWithCovarianceStamped() + self.depth_msg.header.frame_id = self.odom_frame + self.depth_msg.pose.covariance = [100., 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 100., 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + self.depth_msg.pose.pose.orientation.w = 1. + + # TODO is there a reason to have two listeners? + # HAHA is there a reason for even one? + # self.listener_odom = tf.TransformListener() # remove + # self.listener_press = tf.TransformListener() # remove + self.x_base_depth = 0.580 + + # try: + # (trans,quaternion) = self.listener_press.lookupTransform(self.base_frame, self.depth_frame, rospy.Time(10)) + + # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + # print('Could not get tf base to depth.') + + def declare_node_parameters(self): + """ + Declare the parameters for the node + """ + + # TODO This might be a bad way to set defaults + # allows for me to run from the terminal directly for test + default_robot_name = 'sam0' + self.declare_parameter("robot_name", default_robot_name) + + self.declare_parameter('odom_frame', 'odom') + + # def depthCB_old(self, press_msg): + # try: + # + # # # depth_abs is positive, must be manually negated + # depth_abs = - self.pascal_pressure_to_depth(press_msg.fluid_pressure) + # # rospy.loginfo("Depth abs %s", depth_abs) + # # rospy.loginfo("Fluid press %s", press_msg.fluid_pressure) + # + # if press_msg.fluid_pressure > 90000. and press_msg.fluid_pressure < 500000.: + # self.depth_msg.header.stamp = rospy.Time.now() + # self.depth_msg.pose.pose.position.z = depth_abs # = [0., 0., 2.] + # self.pub.publish(self.depth_msg) + # + # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + # rospy.logerr("Depth transform missing tf") + + def depthCB(self, press_msg): + """ + callback for converting pressure message into a depth. + """ + # depth_abs is positive, must be manually negated + depth_abs = - self.pascal_pressure_to_depth(press_msg.fluid_pressure) + # rospy.loginfo("Depth abs %s", depth_abs) + # rospy.loginfo("Fluid press %s", press_msg.fluid_pressure) + + if press_msg.fluid_pressure > 90000. and press_msg.fluid_pressure < 500000.: + self.depth_msg.header.stamp = rcl_time_to_stamp(self.get_clock().now()) + self.depth_msg.pose.pose.position.z = depth_abs # = [0., 0., 2.] + self.pub.publish(self.depth_msg) + + def pascal_pressure_to_depth(self, pressure): + """ + Convert pressure in pascal to depth in meters + + What sensor is this for? The offset by 1 feels a little weird. + + P = pgh + P: pascals, kg/ms^2 + p: density of water, kg/m^3 + g: accel due to gravity, m/s^2 + h: height of water column, m + + p(fresh) = 997.0 # random internet value + p(salt) = 1023.6 # random internet value + g(stockholm) = 9.818 + """ + # TODO check this + return 10. * ((pressure / 100000.) - 1.) # 117000 -> 1.7 + + def simulated_pressure_to_depth(self, pressure: float) -> float: + """ + Convert pressure from simulator in pascals to depth in meters. + + General: + pressure = density * gravity * depth + + Unity: + Unity uses a value of 9806.65 to convert from depth to pressure + pressure = 9806.65 * depth + """ + depth_to_pressure = 9806.65 + return pressure / depth_to_pressure + + +def main(args=None): + rclpy.init(args=args) + depth_node = Press2Depth() + try: + rclpy.spin(depth_node) + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/setup.py b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/setup.py index b2b928c1..c15ddfa3 100644 --- a/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/setup.py +++ b/external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/setup.py @@ -24,6 +24,7 @@ entry_points={ 'console_scripts': [ 'sam_joy_xbox = sam_joy_xbox.controller:main', + 'depth_node = sam_joy_xbox.press_to_depth:main', ], }, ) diff --git a/external_equipment/sam_joy_controllers/sam_joy_teleop/sam_joy_teleop/test_thrust_vec.py b/external_equipment/sam_joy_controllers/sam_joy_teleop/sam_joy_teleop/test_thrust_vec.py new file mode 100755 index 00000000..d911e279 --- /dev/null +++ b/external_equipment/sam_joy_controllers/sam_joy_teleop/sam_joy_teleop/test_thrust_vec.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sam_msgs.msg import ThrusterAngles, Topics +import math +from smarc_msgs.msg import ThrusterRPM + +class CircularThrustVectorPublisher(Node): + + def __init__(self): + super().__init__('circular_thrust_vector_publisher') + + # Publisher for thrust vector angles + self.vector_pub = self.create_publisher( + ThrusterAngles, + "/sam/core/thrust_vector_cmd", + qos_profile=1 + ) + + self.thrust1_pub = self.create_publisher(ThrusterRPM, "/sam/core/thruster1_cmd", qos_profile=1) + self.thrust2_pub = self.create_publisher(ThrusterRPM, "/sam/core/thruster2_cmd", qos_profile=1) + + # Timer to update and send commands + self.timer_period = 0.1 # 10 Hz + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + # Internal state for angle calculation + self.t = 0.0 + self.get_logger().info("Circular thrust vector publisher started.") + + def timer_callback(self): + # Circle radius in radians + radius = 0.1 # Must match actuator limits from your original code + frequency = 0.2 # Hz, how many full cycles per second + omega = 2 * math.pi * frequency + + # Compute current angle vector (simple circular motion) + horizontal = radius * math.cos(omega * self.t) + vertical = radius * math.sin(omega * self.t) + + # Create and publish message + msg = ThrusterAngles() + msg.header.stamp = self.get_clock().now().to_msg() + msg.thruster_horizontal_radians = horizontal + msg.thruster_vertical_radians = vertical + self.vector_pub.publish(msg) + + rpm_msg = ThrusterRPM() + rpm_msg.rpm = 400 + self.thrust1_pub.publish(rpm_msg) + rpm_msg.rpm = 400 + self.thrust2_pub.publish(rpm_msg) + + self.get_logger().info(f"Published vector: h={horizontal:.3f}, v={vertical:.3f}", throttle_duration_sec=1) + + self.t += self.timer_period + +def main(args=None): + rclpy.init(args=args) + node = CircularThrustVectorPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/external_equipment/sam_joy_controllers/sam_joy_teleop/setup.py b/external_equipment/sam_joy_controllers/sam_joy_teleop/setup.py index 068fc62d..d16156bc 100644 --- a/external_equipment/sam_joy_controllers/sam_joy_teleop/setup.py +++ b/external_equipment/sam_joy_controllers/sam_joy_teleop/setup.py @@ -21,6 +21,7 @@ entry_points={ 'console_scripts': [ 'teleop_node = sam_joy_teleop.teleop_node:main', + 'test_thrust_node = sam_joy_teleop.test_thrust_vec:main', ], }, ) From 1adee06a1f50cea63317394b4ef9e603a9ce483b Mon Sep 17 00:00:00 2001 From: ignaciotb Date: Tue, 15 Jul 2025 10:21:47 +0200 Subject: [PATCH 3/3] The submodule had been copied manually? --- .gitmodules | 1 - utilities/serial_ping_pkg | 1 - 2 files changed, 2 deletions(-) delete mode 160000 utilities/serial_ping_pkg diff --git a/.gitmodules b/.gitmodules index 6f7b2165..03a78d67 100644 --- a/.gitmodules +++ b/.gitmodules @@ -48,7 +48,6 @@ path = vehicles/hardware/dji/psdk_ros2 url = https://gitr.sys.kth.se/smarc-project/dji_psdk_wrapper.git branch = smarc - [submodule "utilities/serial_ping_pkg"] path = utilities/serial_ping_pkg url = https://github.com/NinjaTuna007/serial_ping_pkg.git diff --git a/utilities/serial_ping_pkg b/utilities/serial_ping_pkg deleted file mode 160000 index 7398a922..00000000 --- a/utilities/serial_ping_pkg +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7398a922931259e84578c06d605a1a7029ef0f74