Updated code from the summer

asv_setup/config/robots/freya.yaml

@@ -34,8 +34,8 @@
           yaw:   true
       thrusters:
         num: 4
-        min: -200
-        max: 200
+        min: -1000
+        max: 1000
         configuration_matrix: #NED
           [0.70711, 0.70711, 0.70711, 0.70711, 
           -0.70711, 0.70711, -0.70711, 0.70711, 

asv_setup/config/sitaw/landmark_server_params.yaml

@@ -0,0 +1,6 @@
+landmark_server_node:
+  ros__parameters:
+    fixed_frame: map
+    target_frame: base_link
+    wall_width: 0.1
+    use_grid: false

asv_setup/config/sitaw/map_manager_params.yaml

@@ -0,0 +1,25 @@
+map_manager_node:
+  ros__parameters:
+    use_predef_landmask: true
+    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml"
+    # landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml"
+    map_resolution: 0.2
+    map_width: 1000
+    map_height: 1000
+    frame_id: "map"
+    # Map origin for office rosbag
+    # map_origin_lat: 63.414660884931976 
+    # map_origin_lon: 10.398554661537544
+    # use_predef_map_origin: true
+    # Map origin for sim_navigation
+    # map_origin_lat: -33.72242824301795
+    # map_origin_lon: 150.6740063854522
+    # use_predef_map_origin: true
+    # Map origin for sim_maneuvering
+    # map_origin_lat: -33.72213985487166
+    # map_origin_lon: 150.67413507552098
+    # use_predef_map_origin: true
+    #colav
+    map_origin_lat: -33.72213988382845
+    map_origin_lon: 150.67413500672993
+    use_predef_map_origin: true

asv_setup/config/sitaw/seapath_params.yaml

@@ -0,0 +1,4 @@
+seapath_ros_driver_node:
+  ros__parameters:
+    UDP_IP: "10.0.1.10"  # Source IP of the host sender
+    UDP_PORT: 31421  # Port at which to receive UDP segment

asv_setup/launch/sim_freya.launch.py

@@ -0,0 +1,69 @@
+from os import path
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    # Set environment variable
+    set_env_var = SetEnvironmentVariable(
+        name='ROSCONSOLE_FORMAT',
+        value='[${severity}] [${time}] [${node}]: ${message}'
+    )
+
+
+    # Thruster Allocator launch
+    thruster_allocator_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            path.join(get_package_share_directory('thruster_allocator'), 'launch', 'thruster_allocator.launch.py')
+        )
+    )
+
+    # Joystick node
+    joy_node = Node(
+        package='joy',
+        executable='joy_node',
+        name='joystick_driver',
+        output='screen',
+        parameters=[
+            {'deadzone': 0.15},
+            {'autorepeat_rate': 100.0},
+        ],
+        remappings=[
+            ('/joy', '/joystick/joy'),
+        ],
+    )
+
+    # Joystick interface launch
+    joystick_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            path.join(get_package_share_directory('joystick_interface'), 'launch', 'joystick_interface.launch.py')
+        )
+    )
+
+    # Vortex Sim Interface launch
+    vortex_sim_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            path.join(get_package_share_directory('vortex_sim_interface'), 'launch', 'vortex_sim_interface.launch.py')
+        )
+    )
+
+    # Controller
+    controller_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            path.join(get_package_share_directory('asv_setup'), 'launch', 'hybridpath.launch.py')
+        )
+    )
+
+    # Return launch description
+    return LaunchDescription([
+        set_env_var,
+        thruster_allocator_launch,
+        joy_node,
+        joystick_interface_launch,
+        controller_launch,
+        vortex_sim_interface_launch, 
+    ])

asv_setup/launch/sitaw.launch.py

@@ -0,0 +1,60 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    enable_tf = LaunchConfiguration('enable_tf')
+    enable_tf_arg = DeclareLaunchArgument(
+        'enable_tf',
+        default_value='True',
+        description='enable tf launch file',
+    )
+    enable_seapath = LaunchConfiguration('enable_seapath')
+    enable_seapath_arg = DeclareLaunchArgument(
+        'enable_seapath',
+        default_value='True',
+        description='enable seapath launch file',
+    )
+
+    seapath_ros_driver_node = Node(
+            package='seapath_ros_driver',
+            executable='seapath_ros_driver_node',
+            name='seapath_ros_driver_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','seapath_params.yaml')],
+            output='screen',
+            condition=IfCondition(enable_seapath),
+
+        )
+    map_manager_node = Node(
+            package='map_manager',
+            executable='map_manager_node',
+            name='map_manager_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','map_manager_params.yaml')],
+            output='screen',
+        )
+    landmark_server_node = Node(
+            package='landmark_server',
+            executable='landmark_server_node',
+            name='landmark_server_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','landmark_server_params.yaml')],
+            # arguments=['--ros-args', '--log-level', 'DEBUG'],
+            output='screen',
+        )
+    tf_launch = IncludeLaunchDescription(
+        launch_description_source=PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('asv_setup'), 'launch', 'tf.launch.py')),
+        condition=IfCondition(enable_tf),
+    )
+    return LaunchDescription([
+        enable_tf_arg,
+        tf_launch,
+        enable_seapath_arg,
+        seapath_ros_driver_node,
+        map_manager_node,
+        landmark_server_node
+    ])

asv_setup/launch/tf.launch.py

@@ -36,9 +36,9 @@ def generate_launch_description():
     )
 
     # base_link (NED) to seapath_frame (NED) tf.
-    tf_base_link_to_seapath = Node(
+    tf_seapath_to_base_link = Node(
         package='tf2_ros',
-        name='base_link_ned_to_seapath_frame',
+        name='seapath_to_base_link',
         executable='static_transform_publisher',
         arguments=['--x'              , '0',
                    '--y'              , '0',
@@ -50,8 +50,23 @@ def generate_launch_description():
                    '--child-frame-id' , 'base_link'],
     )
 
+    os_sensor_to_os_lidar = Node(
+        package='tf2_ros',
+        name='os_sensor_to_os_lidar',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '0',
+                   '--y'              , '0',
+                   '--z'              , '0.036180000000000004',
+                   '--roll'           , '0',
+                   '--pitch'          , '0',
+                   '--yaw'            , str(math.pi),
+                   '--frame-id'       , 'os_sensor',
+                   '--child-frame-id' , 'os_lidar'],
+    )
+
     return LaunchDescription([
         tf_base_link_to_lidar,
         tf_base_link_to_zed_camera_link,
-        tf_base_link_to_seapath,
+        tf_seapath_to_base_link,
+        os_sensor_to_os_lidar,
     ])

guidance/hybridpath_guidance/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(ament_cmake_python REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(vortex_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 
 ament_python_install_package(${PROJECT_NAME})
 

guidance/hybridpath_guidance/README.md

@@ -9,7 +9,7 @@ Or alternatively, run it together with the hybridpath controller using the launc
 
 To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered):
 
-`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"`
+`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 5.0, y: 5.0, z: 0.0}]"`
 
 ## Configuration
 

guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml

@@ -4,4 +4,4 @@
       lambda_val: 0.15 # Curvature constant
       path_generator_order: 1 # Differentiability order
       dt: 0.1 # Time step
-      mu: 0.03 # Tuning parameter 
+      mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py

@@ -66,6 +66,49 @@ class HybridPathGenerator:
         path (Path): The path object.
     """
     def __init__(self, WP: list[Point], r: int, lambda_val: float):
+        self.WP = WP
+        self.r = r
+        self.lambda_val = lambda_val
+        self.order = 2*r + 1
+
+    def create_path(self, WP: list[Point]) -> None:
+        """
+        Create a path object.
+
+        Args:
+            WP (list[Point]): A list of waypoints.
+
+        Returns:
+            None
+        """
+        self.update_waypoints(WP)
+        self._initialize_path()
+        self._calculate_subpaths()
+
+    def _initialize_path(self):
+        """
+        Initialize the path object.
+        """
+        self.path = Path()
+
+        self.path.coeff.a = []
+        self.path.coeff.b = []
+        self.path.coeff.a_der = []
+        self.path.coeff.b_der = []
+        self.path.LinSys.A = None
+        self.path.LinSys.bx = []
+        self.path.LinSys.by = []
+
+        self.path.NumSubpaths = len(self.WP) - 1
+        self.path.Order = self.order
+
+    def update_waypoints(self, WP: list[Point]) -> None:
+        """
+        Updates the waypoints for the path.
+
+        Args:
+            WP (list[Point]): A list of waypoints.
+        """
         # Convert the waypoints to a numpy array
         WP_arr = np.array([[wp.x, wp.y] for wp in WP])
 
@@ -77,13 +120,6 @@ def __init__(self, WP: list[Point], r: int, lambda_val: float):
 
         else:
             self.WP = WP_arr
-        self.r = r
-        self.lambda_val = lambda_val
-        self.order = 2*r + 1
-        self.path = Path()
-        self.path.NumSubpaths = len(self.WP) - 1
-        self.path.Order = self.order
-        self._calculate_subpaths()
 
     def _construct_A_matrix(self) -> np.ndarray:
         """
@@ -280,6 +316,12 @@ def _calculate_subpaths(self) -> None:
             for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)):
                 self._append_derivatives(k, a, b)
 
+    def get_path(self) -> Path:
+        """
+        Get the hybrid path.
+        """
+        return self.path
+
     @staticmethod
     def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float:
         """
@@ -295,7 +337,9 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo
             float: The updated position along the hybrid path.
 
         """
-        signals = HybridPathSignals(path, s)
+        signals = HybridPathSignals()
+        signals.update_path(path)
+        signals.update_s(s)
         v_s = signals.get_vs(u_desired)
         s_new = s + (v_s + w) * dt
         return s_new
@@ -314,13 +358,12 @@ class HybridPathSignals:
         Path (Path): The path object.
         s (float): The path parameter.
     """
-    def __init__(self, path: Path, s: float):
-        if not isinstance(path, Path):
-            raise TypeError("path must be an instance of Path")
-        self.path = path
-        self.s = self._clamp_s(s, self.path.NumSubpaths)
+    def __init__(self):
+        self.path = None
+        self.s = None
 
-    def _clamp_s(self, s: float, num_subpaths: int) -> float:
+    @staticmethod
+    def _clamp_s(s: float, num_subpaths: int) -> float:
         """
         Ensures s is within the valid range of [0, num_subpaths].
 
@@ -506,4 +549,29 @@ def get_w(self, mu: float, eta: np.ndarray) -> float:
         w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error)
 
         return w
+
+    def update_path(self, path: Path) -> None:
+        """
+        Update the path object.
+
+        Args:
+            path (Path): The new path object.
+
+        Returns:
+            None
+        """
+        self.path = path
+        self.update_s(0.)
+
+    def update_s(self, s: float) -> None:
+        """
+        Update the path parameter.
+
+        Args:
+            s (float): The new path parameter.
+
+        Returns:
+            None
+        """
+        self.s = self._clamp_s(s, self.path.NumSubpaths)