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/land_polygon_grillstad.yaml

@@ -0,0 +1,8 @@
+63.437946025438066 10.507175268806458
+63.43787563495969 10.507121157686353
+63.4378360402396 10.50719002638467
+63.43763366637102 10.50697358190425
+63.43760726967404 10.507101480915406
+63.43713432473124 10.506752218231092
+63.437239913116464 10.505876601923934
+63.438056010214204 10.506461985859616

asv_setup/config/sitaw/land_polygon_office.yaml

@@ -0,0 +1,16 @@
+63.41489120183045 10.39705552070149
+63.41497265487507 10.39783732516908
+63.41538547016246 10.397651181248118
+63.415431749532054 10.398250978326427
+63.415048554100395 10.398449531842004
+63.41512075073269 10.399185834462275
+63.41495229164123 10.399305793877936
+63.41486158248972 10.398540535536645
+63.41453021396673 10.398747362115373
+63.4145246602717 10.398631539231284
+63.414280296625655 10.39877218130482
+63.41424142039905 10.398197203415958
+63.4144598670836 10.398110336252891
+63.41444505719146 10.397990376837232
+63.41478938520484 10.397841461700548
+63.41472089017681 10.397167207053899

asv_setup/config/sitaw/land_polygon_sim_maneuvering.yaml

@@ -0,0 +1,4 @@
+-33.722464875303054 150.6733186790048
+-33.720864277234156 150.673353826611
+-33.72115033638088 150.67502276015122
+-33.72271743627459 150.67461276680072

asv_setup/config/sitaw/land_polygon_sim_navigation.yaml

@@ -0,0 +1,5 @@
+-33.722576743831986 150.67377683501326
+-33.72213785615945 150.6737273159938
+-33.722119332734295 150.6744244880215
+-33.72265441348879 150.6743192836287
+

asv_setup/config/sitaw/landmark_server_params.yaml

@@ -0,0 +1,5 @@
+landmark_server_node:
+  ros__parameters:
+    fixed_frame: map
+    target_frame: base_link
+    wall_width: 0.1

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,7 @@
+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 semgents
+    use_predef_map_origin: false
+    map_origin_lat: 0.0
+    map_origin_lon: 0.0

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