Merged with upstream

Author: pool man

.gitmodules

@@ -29,7 +29,30 @@
 	path = drivers/nmea_navsat_driver
 	url = [email protected]:smarc-project/nmea_navsat_driver.git
 	branch = ros2
+[submodule "drivers/stim300"]
+	path = drivers/stim300
+	url = [email protected]:smarc-project/stim300.git
+	branch = humble
+[submodule "drivers/dvl-a50-ros-driver"]
+	path = drivers/dvl-a50-ros-driver
+	url = [email protected]:smarc-project/dvl-a50-ros-driver.git
+	branch = humble
+[submodule "drivers/ublox"]
+	path = drivers/ublox
+	url = https://github.com/KumarRobotics/ublox.git
+	branch = ros2
 [submodule "external_packages/smarc_modelling"]
 	path = external_packages/smarc_modelling
 	url = https://github.com/smarc-project/smarc_modelling.git
-	branch = master
+[submodule "vehicles/hardware/dji/psdk_ros2"]
+	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
+[submodule "external_equipment/PlayStation-JoyInterface-ROS2"]
+	path = external_equipment/PlayStation-JoyInterface-ROS2
+	url = https://github.com/HarvestX/PlayStation-JoyInterface-ROS2
+	branch = humble

All Topics.msg.md

@@ -108,6 +108,13 @@
 -  TELEOP_ENABLE :  'teleop/enable'
 -  ASSIST_ENABLE :  'teleop/drive_assist_enable'
 -  ELEVATOR_PID_CTRL :  'ctrl/elevator_pid/ctrl_effort'
+-  CONTROL_YAW_TOPIC :  "ctrl/yaw"              
+-  CONTROL_YAW_RATE_TOPIC :  "ctrl/yawrate"     
+-  CONTROL_PITCH_TOPIC :  "ctrl/pitch"          
+-  CONTROL_PITCH_RATE_TOPIC :  "ctrl/pitchrate" 
+-  CONTROL_ROLL_TOPIC :  "ctrl/roll"            
+-  CONTROL_ROLL_RATE_TOPIC :  "ctrl/rollrate"   
+-  CONTROL_SURGE_RATE_TOPIC :  "ctrl/surgerate" 
 ## Topics in file: [messages/lolo_msgs/msg/Topics.msg](messages/lolo_msgs/msg/Topics.msg)
 
 -  RUDDER_CMD :  "actuators/rudder_cmd"
@@ -128,7 +135,7 @@
 -  THRUSTER_STRB_FB :  "actuators/thruster_strb_fb"
 -  VERTICAL_THRUSTER_BACK_PORT_FB :  "actuators/vertical_thruster_back_port_fb"
 -  VERTICAL_THRUSTER_BACK_STRB_FB :  "actuators/vertical_thruster_back_strb_fb"
--  VERTICAL_THRUSTER_FRONT_PORT_FB :  "actuators/vertical_thruster_front_strb_fb"
+-  VERTICAL_THRUSTER_FRONT_PORT_FB :  "actuators/vertical_thruster_front_port_fb"
 -  VERTICAL_THRUSTER_FRONT_STRB_FB :  "actuators/vertical_thruster_front_strb_fb"
 -  HEARTBEAT_TOPIC :  "heartbeat"
 -  LEAK_TOPIC :  "leak"
@@ -151,6 +158,28 @@
 -  SATELITE_RECEIVED_TOPIC :  "communication/satelite_received"
 -  USBL_RECEIVED_TOPIC :  "communication/usbl/received"
 -  USBL_RECEIVED_CHR_TOPIC :  "communication/usbl/received/chr"
+-  INS_ODOM_TOPIC :  "ins_odom"
+-  INS_RAW_TOPIC :  "/ix/ins"
+-  INS_IMU_TOPIC :  "/standard/imu"
+-  GPS_RAW_TOPIC :  sensors/gps/raw
+-  GPS_TOPIC :  sensors/gps
+-  SVS_RAW_TOPIC :  sensors/svs/raw
+-  SVS_TOPIC :  sensors/svs
+-  CTD_RAW_TOPIC :  sensors/ctd/raw
+-  CTD_TOPIC :  sensors/ctd
+-  DO_RAW_TOPIC :  sensors/do/raw
+-  DO_TOPIC :  sensors/do
+-  TURB_RAW_TOPIC :  sensors/turbidity/raw
+-  TURB_TOPIC :  sensors/turbidity
+-  FLUORESCENCE_RAW_TOPIC :  sensors/fluorescence/raw
+-  FLUORESCENCE_TOPIC :  sensors/fluorescence
+-  DVL_TOPIC :  sensors/dvl
+-  FLS_WC_TOPIC :  sensors/fls/watercolumn
+-  FLS_IMAGE_TOPIC :  sensors/fls/watercolumn/image
+-  MBES_WC_TOPIC :  sensors/fls/watercolumn
+-  MBES_IMAGE_TOPIC :  sensors/fls/watercolumn/image
+-  MBES_BATHY_TOPIC :  sensors/fls/bathymetry
+-  MBES_BATHY_POINTCLOUD_TOPIC :  sensors/fls/bathymetry/points
 ## Topics in file: [messages/smarc_msgs/msg/Topics.msg](messages/smarc_msgs/msg/Topics.msg)
 
 -  GPS_TOPIC :  'core/gps'
@@ -209,9 +238,12 @@
 -  'core/imu': 
     - [ STIM_IMU_TOPIC ](messages/sam_msgs/msg/Topics.msg)
     - [ IMU_TOPIC ](messages/drone_msgs/msg/Topics.msg)
--  "actuators/vertical_thruster_front_strb_fb": 
-    - [ VERTICAL_THRUSTER_FRONT_PORT_FB ](messages/lolo_msgs/msg/Topics.msg)
-    - [ VERTICAL_THRUSTER_FRONT_STRB_FB ](messages/lolo_msgs/msg/Topics.msg)
+-  sensors/fls/watercolumn: 
+    - [ FLS_WC_TOPIC ](messages/lolo_msgs/msg/Topics.msg)
+    - [ MBES_WC_TOPIC ](messages/lolo_msgs/msg/Topics.msg)
+-  sensors/fls/watercolumn/image: 
+    - [ FLS_IMAGE_TOPIC ](messages/lolo_msgs/msg/Topics.msg)
+    - [ MBES_IMAGE_TOPIC ](messages/lolo_msgs/msg/Topics.msg)
 -  'core/gps': 
     - [ GPS_TOPIC ](messages/smarc_msgs/msg/Topics.msg)
     - [ GPS_TOPIC ](messages/drone_msgs/msg/Topics.msg)

README.md

@@ -6,6 +6,10 @@ Each of the following directories also include more detailed readmes within.
 [This document](/documentation/Installing%20ROS2.md) has step-by-step instructions to get things running.
 
 
+## [Making a new Action for a robot? Check this example out](/behaviours/alars/alars/SuperSimpleActionServer.py)
+[This example](/behaviours/alars/alars/SuperSimpleActionServer.py) implements an action server that hides ALL of the ROS from your beautiful control implementations. 
+It works with [the bt](./behaviours/wasp_bt/launch/wasp_bt.launch) out of the box.
+
 ## Stuff in this repo
 
 > Add more directories as needed.

utilities/utm_latlon_converter/LICENSE renamed to behaviours/alars/LICENSE

@@ -0,0 +1,253 @@
+import rclpy, math
+
+
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.time import Time, Duration
+
+from tf2_ros import Buffer, TransformListener
+from tf2_geometry_msgs import do_transform_pose_stamped
+from tf_transformations import euler_from_quaternion, quaternion_from_euler
+
+
+from smarc_action_base.gentler_action_server import GentlerActionServer
+from smarc_utilities.georef_utils import convert_latlon_to_utm
+
+from smarc_msgs.msg import Topics as SmarcTopics
+
+from geographic_msgs.msg import GeoPoint
+from geometry_msgs.msg import PointStamped, PoseStamped
+
+class SearchAndTrackAuvAction():
+    def __init__(self,
+                 node: Node,
+                 action_name: str):
+        self._node = node
+
+        self.log = node.get_logger().info
+        
+        self.ODOM_FRAME = "Quadrotor/odom"
+        self.BASE_FRAME = "Quadrotor/base_link"
+        self.SETPOINT_TOPIC = "move_to_setpoint"
+        self.POINT_REACHED_DISTANCE = 0.3
+        self.SPIRAL_ARM_DISTANCE = 0.5  # Distance between spiral arms in meters
+
+        self._as = GentlerActionServer(
+            node,
+            action_name,
+            self._on_goal_received,
+            self._on_cancel_received,
+            self._prepare_loop,
+            self._loop_inner,
+            self._give_feedback,
+            loop_frequency=10.0
+        )
+
+        self._search_center_odom : PointStamped|None = None
+        self._search_radius : float = 0.0
+        self._keep_following : bool = False
+        self._spiral_position : float|None = None
+
+
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=True)
+
+        self._feedback_str = "No feedback yet"
+
+        self._setpoint_pub = self._node.create_publisher(
+            PoseStamped,
+            self.SETPOINT_TOPIC,
+            10)
+        
+        self._auv_detection_sub = self._node.create_subscription(
+            PointStamped,
+            "alars_detection/auv",
+            self._auv_detection_callback,
+            10)
+        
+    @property
+    def now_stamp(self):
+        return self._node.get_clock().now().to_msg()
+    
+
+    def _auv_detection_callback(self, msg: PointStamped):
+        #TODO
+        pass
+
+
+    def _on_goal_received(self, goal_request: dict) -> bool:
+        self.log(f"Received goal request: {goal_request}")
+        gp = goal_request['search_center']
+        alt : float = gp['altitude']
+        radius : float = gp['tolerance']
+
+        try:
+            if alt < 2.0:
+                self._node.get_logger().error("Search center altitude must be > 2.0 meters")
+                return False
+            if alt > 10.0:
+                self._node.get_logger().error("Search center altitude must be < 10.0 meters")
+                return False
+            if radius > 100.0:
+                self._node.get_logger().error("Search tolerance must be < 100.0 meters")
+                return False
+            if radius < 0.0:
+                self._node.get_logger().error("Search tolerance must be >= 0.0 meters")
+                return False
+        except:
+            self._node.get_logger().error("Invalid goal request format!")
+            return False
+        
+        center_gp = GeoPoint()
+        center_gp.latitude = float(gp['latitude'])
+        center_gp.longitude = float(gp['longitude'])
+        center_gp.altitude = alt
+        center_utm : PointStamped = convert_latlon_to_utm(center_gp)
+        
+        try:
+            tf = self._tf_buffer.lookup_transform(
+                self.ODOM_FRAME, 
+                center_utm.header.frame_id, 
+                Time(seconds=0),
+                timeout=Duration(seconds=1)
+            )
+            ps = PoseStamped()
+            ps.header = center_utm.header
+            ps.pose.position = center_utm.point
+            ps.pose.orientation.w = 1.0  # Set orientation to identity
+
+            pnt = do_transform_pose_stamped(ps, tf)
+            self._search_center_odom = PointStamped()
+            self._search_center_odom.header = pnt.header
+            self._search_center_odom.point = pnt.pose.position
+
+        except Exception as e:
+            self.log(f"Failed to transform search center from {center_utm.header.frame_id} to {self.ODOM_FRAME}: {e}")
+            return False
+        
+        self._search_radius = radius
+        self._keep_following = bool(goal_request['keep_following'])
+        self._spiral_position = None
+
+        self.log(f"Search center set to: {format_point_stamped(self._search_center_odom)}")
+        self.log(f"Search radius set to: {self._search_radius} meters")
+        return True
+
+        
+    
+
+    def _on_cancel_received(self) -> bool:
+        self._node.get_logger().info("Received cancel request")
+        return True
+    
+
+    def _prepare_loop(self) -> None:
+        if self._search_center_odom is None:
+            self._node.get_logger().error("Search center not set, cannot start action")
+            return
+        self._current_setpoint_odom: PoseStamped = PoseStamped()
+        self._current_setpoint_odom.pose.position.x = self._search_center_odom.point.x
+        self._current_setpoint_odom.pose.position.y = self._search_center_odom.point.y
+        self._current_setpoint_odom.pose.position.z = self._search_center_odom.point.z
+        self._current_setpoint_odom.header = self._search_center_odom.header
+        
+
+    def _update_current_setpoint_on_spiral(self):
+        if self._spiral_position is None:
+            self._spiral_position = 0
+
+        if self._search_center_odom is None:
+            self.log("Search center not set, cannot update spiral position")
+            return
+        
+        spiral_x, spiral_y = point_on_spiral(self._spiral_position, base_radius=self.POINT_REACHED_DISTANCE, arm_distance=self.SPIRAL_ARM_DISTANCE)
+        self._current_setpoint_odom.pose.position.x = spiral_x + self._search_center_odom.point.x
+        self._current_setpoint_odom.pose.position.y = spiral_y + self._search_center_odom.point.y
+
+
+    def _loop_inner(self) -> bool | None:
+        if self._search_center_odom is None:
+            self.log("Search center not set, cannot continue action")
+            return False
+        
+        tf = self._tf_buffer.lookup_transform(
+            self.BASE_FRAME, 
+            self._current_setpoint_odom.header.frame_id, 
+            Time(seconds=0),
+            timeout=Duration(seconds=1)
+        )
+        current_setpoint_in_base = do_transform_pose_stamped(self._current_setpoint_odom, tf)
+        # Calculate the magnitude (distance from origin) of the setpoint in the base frame
+        pos = current_setpoint_in_base.pose.position
+        dist_to_current_setpoint = math.sqrt(pos.x ** 2 + pos.y ** 2 + pos.z ** 2)
+        if dist_to_current_setpoint < self.POINT_REACHED_DISTANCE:
+            if self._spiral_position is not None:
+                self._spiral_position += math.pi / 16
+                self._update_current_setpoint_on_spiral()
+            else:
+                self.log("Reached search center, starting search")
+                self._spiral_position = 0
+                self._update_current_setpoint_on_spiral()
+
+        # Check distance between current_setpoint_odom and search_center
+        dx = self._current_setpoint_odom.pose.position.x - self._search_center_odom.point.x
+        dy = self._current_setpoint_odom.pose.position.y - self._search_center_odom.point.y
+        dz = self._current_setpoint_odom.pose.position.z - self._search_center_odom.point.z
+        dist_from_center = math.sqrt(dx**2 + dy**2 + dz**2)
+        if dist_from_center > self._search_radius:
+            self.log(f"Current setpoint is outside search radius of {self._search_radius}m, search failed.")
+            return False
+        
+        self._current_setpoint_odom.header.stamp = self.now_stamp
+        self._setpoint_pub.publish(self._current_setpoint_odom)
+        self._feedback_str = f"Remaining: {dist_to_current_setpoint:.2f}m"
+        return None
+    
+
+
+    def _give_feedback(self) -> str:
+        return self._feedback_str
+
+
+def point_on_spiral(T_rad:float, base_radius:float=0, arm_distance:float=1) -> tuple[float, float]:
+    r = base_radius + arm_distance * T_rad
+    x = r * math.cos(T_rad)
+    y = r * math.sin(T_rad)
+    return x, y
+
+
+def main():
+    rclpy.init()
+    node = Node("search_auv_action_node")
+    
+    SearchAndTrackAuvAction(node, "alars_search_and_follow")
+
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down Search AUV Action server")
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+def format_point_stamped(point: PointStamped|None) -> str:
+        if( point is None):
+            return "None"
+        return f"(x={point.point.x:.3f}, y={point.point.y:.3f}, z={point.point.z:.3f}, frame_id={point.header.frame_id})"
+
+def format_pose_stamped(pose: PoseStamped|None) -> str:
+        if( pose is None):
+            return "None"
+        rpy = euler_from_quaternion([
+            pose.pose.orientation.x,
+            pose.pose.orientation.y,
+            pose.pose.orientation.z,
+            pose.pose.orientation.w
+        ])
+        return f"(x={pose.pose.position.x:.3f}, y={pose.pose.position.y:.3f}, z={pose.pose.position.z:.3f}, " \
+               f"roll={math.degrees(rpy[0]):.3f}, pitch={math.degrees(rpy[1]):.3f}, yaw={math.degrees(rpy[2]):.3f}, " \
+               f"frame_id={pose.header.frame_id})"