Merge pull request #186 from ignaciotb/humble

Latest assisted diving from Sam

Author: ignaciotb

.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

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

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)
 

external_equipment/sam_joy_controllers/sam_joy_controllers/sam_joy_xbox/launch/joy_depth_control.launch

@@ -7,7 +7,7 @@
     <group>
         <push-ros-namespace namespace="$(var robot_name)"/>
 
-	<node pkg="sam_dead_reckoning" exec="depth_node" name="depth_node" output="screen">
+	<node pkg="sam_joy_xbox" exec="depth_node" name="depth_node" output="screen">
 	     <param name="robot_name" value="$(var robot_name)" />
 	     <param name="simulation" value="false" />
 	     <param name="use_sim_time" value="false" />

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()

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',
         ],
     },
 )

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()

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',
         ],
     },
 )