diff --git a/behaviours/floatsam/floatsam_move_to/README.md b/behaviours/floatsam/floatsam_move_to/README.md
new file mode 100644
index 000000000..0312dbc76
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/README.md
@@ -0,0 +1,7 @@
+launchfile:
+ros2 launch floatsam_move_to floatsam_all.launch.py
+
+send goal:
+ros2 action send_goal /floatsam_usv/move_to smarc_msgs/action/BaseAction   "{goal: {data: '{\"latitude\": 58.8394970495000, \"longitude\": 17.650843044440000, \"tolerance\": 1.0, \"speed\": 2.0}'}}"
+
+ros2 action send_goal /floatsam_usv/loiter_heading smarc_msgs/action/BaseAction "{goal: {data: '{\"duration\": 15, \"heading\": 3.14}'}}"
\ No newline at end of file
diff --git a/behaviours/floatsam/floatsam_move_to/floatsam_move_to/README.md b/behaviours/floatsam/floatsam_move_to/floatsam_move_to/README.md
new file mode 100644
index 000000000..c45d031c3
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/floatsam_move_to/README.md
@@ -0,0 +1,73 @@
+# floatsam_move_to
+
+A ROS 2 action server that commands a Floatsam USV to navigate to a specific geographic coordinate (latitude and longitude). It features dynamic speed profiling and strictly relies on a Reciprocal Velocity Obstacles (RVO) service to ensure safe, collision-free routing.
+
+---
+
+## Overview
+
+    Goal (JSON via BaseAction)
+        └── waypoint: 
+            ├── latitude: <float>
+            ├── longitude: <float>
+            └── tolerance: <float>
+        └── speed: <float> or <string> ("slow", "standard", "fast")
+        └── constant_speed: <boolean> (optional)
+
+    Action server: /move_to
+    Action type:   smarc_action_base/action/BaseAction
+
+When a goal is accepted, the server:
+
+1. Converts the target GPS coordinate (latitude/longitude) into the local map frame.
+2. Continuously calculates the Euclidean distance and heading error between the vehicle's current position and the target.
+3. **Speed Profiling**: Automatically reduces speed linearly as the vehicle approaches within 10 meters of the target to prevent overshooting (unless `constant_speed` is explicitly set to `true`).
+4. **Collision Avoidance (RVO)**: Sends its preferred velocity vector to the `get_safe_velocity` RVO service. The RVO service evaluates nearby obstacles/agents and returns a safe velocity and heading.
+5. Publishes the RVO-adjusted speed and heading to the low-level controllers (`velocity_setpoint` and `yaw_setpoint`).
+6. Declares success once the vehicle arrives within the defined `tolerance` radius.
+
+---
+
+## Core Functionalities
+
+### 1. Point-to-Point Navigation
+The server handles the core routing logic, translating high-level geographic goals into continuous heading and speed setpoints for the vehicle's low-level PID controllers. 
+
+### 2. RVO-Based Collision Avoidance
+Instead of blindly following the direct vector to the target, the server passes its desired speed and heading to the `get_safe_velocity` service. If a potential collision is detected, the RVO service returns an altered, safe velocity and angle. The server immediately applies these safe setpoints, allowing decentralized collision avoidance without needing complex behavior trees.
+
+### 3. Smart Speed Scaling
+The server supports named speed presets (`slow` = 1.0 m/s, `standard` = 2.0 m/s, `fast` = 5.0 m/s) alongside raw float values. To ensure smooth arrivals, it automatically scales down the velocity when the vehicle is within 10 meters of the goal. This feature can be bypassed by setting `"constant_speed": true` in the goal request.
+
+### 4. Dynamic Controller Tuning
+The server actively publishes PID tuning parameters (for yaw, yaw rate, and velocity) to the `captain_parameters` topic, ensuring the lower-level controllers are appropriately tuned for transit maneuvers.
+
+---
+
+## Parameters
+
+| Parameter | Default | Description |
+|---|---|---|
+| `robot_name` | `floatsam_usv` | Base name of the robot executing the server. |
+| `yaw_p_gain`, `yaw_i_gain`, `yaw_d_gain` | `0.3`, `0.0`, `0.1` | PID gains for yaw control. |
+| `yawrate_p_gain`, `yawrate_i_gain`, `yawrate_d_gain`| `300.0`, `0.0`, `30.0` | PID gains for yaw rate control. |
+| `velocity_p_gain`, `velocity_i_gain`, `velocity_d_gain`| `500.0`, `10.0`, `0.0` | PID gains for velocity control. |
+
+---
+
+## Running and Activating the Server
+
+### 1. Launch the Server
+
+```bash
+ros2 run floatsam_go_to_formation floatsam_move_to_action_server \
+    --ros-args -p robot_name:=floatsam_usv_0
+
+```
+#### 2. Send a Move Mission (Terminal Command)
+You can activate the server using the ROS 2 CLI. The goal string is a YAML representation containing the JSON payload.
+
+#### Example: Move to coordinates at standard speed with a 1.0m tolerance
+```bash
+ros2 action send_goal /floatsam_usv_0/move_to smarc_msgs/action/BaseAction "{goal: {data: '{\"waypoint\": {\"latitude\": 58.8405, \"longitude\": 17.6520, \"tolerance\": 1.0}, \"speed\": \"standard\", \"constant_speed\": false}'}}"
+```
\ No newline at end of file
diff --git a/behaviours/floatsam/floatsam_move_to/floatsam_move_to/__init__.py b/behaviours/floatsam/floatsam_move_to/floatsam_move_to/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/behaviours/floatsam/floatsam_move_to/floatsam_move_to/floatsam_move_to_server.py b/behaviours/floatsam/floatsam_move_to/floatsam_move_to/floatsam_move_to_server.py
new file mode 100644
index 000000000..85e1532b2
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/floatsam_move_to/floatsam_move_to_server.py
@@ -0,0 +1,328 @@
+#!/usr/bin/python
+
+import numpy as np
+import rclpy
+import json
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.time import Time, Duration
+
+import traceback
+
+from floatsam_controllers.floatsam_common import FloatSam
+
+from smarc_msgs.msg import FloatStamped
+from floatsam_msgs.msg import Topics as FloatsamTopics
+from floatsam_msgs.srv import GetSafeVelocity
+from geometry_msgs.msg import  PointStamped, PoseStamped
+from geographic_msgs.msg import GeoPoint
+from geometry_msgs.msg import PointStamped
+from nav_msgs.msg import Odometry
+from tf_transformations import euler_from_quaternion
+from std_msgs.msg import String
+from std_msgs.msg import Bool
+
+
+
+from tf2_geometry_msgs import do_transform_pose_stamped
+from tf2_ros import Buffer, TransformListener
+
+from smarc_action_base.gentler_action_server import GentlerActionServer
+import time
+
+class MoveToActionFloatSam():
+    def __init__(self,
+                 node: Node):
+        self._node : Node = node
+        
+        self._node.declare_parameter('use_sim', True)
+        self._use_sim = self._node.get_parameter('use_sim').get_parameter_value().bool_value
+        
+        self._robot_name : str = self._node.get_parameter('robot_name').value
+        
+        self.MAP_FRAME : str = self._robot_name + '/map'
+        self._floatsam = FloatSam(node, self._robot_name, use_sim=self._use_sim)
+        
+        self._node.get_logger().info(f"FloatSam move_to server initialized for robot: {self._robot_name}")
+
+        self._default_goal_tolerance = 1  
+        self._default_speed_threshold = 10   
+
+        self.declare_node_parameters()
+
+        self._loiter_yaw_p_gain = str(self._node.get_parameter('yaw_p_gain').value)
+        self._loiter_yaw_i_gain = str(self._node.get_parameter('yaw_i_gain').value)
+        self._loiter_yaw_d_gain = str(self._node.get_parameter('yaw_d_gain').value)
+        self._loiter_yaw_threshold = str(self._node.get_parameter('yaw_threshold').value)
+
+        self._loiter_yawrate_p_gain = str(self._node.get_parameter('yawrate_p_gain').value)
+        self._loiter_yawrate_i_gain = str(self._node.get_parameter('yawrate_i_gain').value)
+        self._loiter_yawrate_d_gain = str(self._node.get_parameter('yawrate_d_gain').value)
+
+        self._loiter_velocity_p_gain = str(self._node.get_parameter('velocity_p_gain').value)
+        self._loiter_velocity_i_gain = str(self._node.get_parameter('velocity_i_gain').value)
+        self._loiter_velocity_d_gain = str(self._node.get_parameter('velocity_d_gain').value)
+
+        self._yaw_reference_publisher = self._node.create_publisher(FloatStamped, FloatsamTopics.YAW_SETPOINT, 10)
+
+        self._speed_reference_publisher = self._node.create_publisher(FloatStamped, FloatsamTopics.VELOCITY_SETPOINT, 10)
+
+        self._move_on_place_publisher = self._node.create_publisher(Bool, 'move_on_place', 1)
+
+        self._rvo_client = self._node.create_client(GetSafeVelocity, 'get_safe_velocity')
+
+        self._captain_parameters_publisher = self._node.create_publisher(
+            String, 
+            'captain_parameters',
+            10
+        )
+        self._as = GentlerActionServer(
+            node,
+            "move_to",
+            self._on_goal_received,
+            self._on_cancel_received,
+            self._prepare_loop,
+            self._loop_inner,
+            self._give_feedback,
+            loop_frequency=10
+        )
+
+        self._initial_pos_deadline = int(self._node.get_clock().now().nanoseconds * 1e-9) + 5
+        self._initial_pos_timer = self._node.create_timer(0.5, self._check_initial_position)
+
+
+    def declare_node_parameters(self) -> None:
+        self._node.declare_parameter("yaw_p_gain", 0.3)
+        self._node.declare_parameter("yaw_i_gain", 0.0)
+        self._node.declare_parameter("yaw_d_gain", 0.1)
+        self._node.declare_parameter("yaw_threshold", 0.5)
+
+        self._node.declare_parameter("yawrate_p_gain", 300.0)
+        self._node.declare_parameter("yawrate_i_gain", 0.0)
+        self._node.declare_parameter("yawrate_d_gain", 30.0)
+
+        self._node.declare_parameter("velocity_p_gain", 500.0)
+        self._node.declare_parameter("velocity_i_gain", 10.0)
+        self._node.declare_parameter("velocity_d_gain", 0.0)
+    
+    
+    @property
+    def now_stamp(self):
+        return self._node.get_clock().now().to_msg()
+    
+    @property
+    def now_time(self):
+        return self.now_stamp.sec + self.now_stamp.nanosec * 1e-9
+    
+    def log(self, msg: str):
+        self._node.get_logger().info(msg)
+
+    def _check_initial_position(self):
+        """Timer callback: print the first floatsam position received from odom_gt (or timeout)."""
+        if self._floatsam.floatsam_in_map is not None:
+            p = self._floatsam.floatsam_in_map.pose.position
+            self._node.get_logger().info(f"Floatsam position from odom_gt: [{p.x:.2f}, {p.y:.2f}, {p.z:.2f}]")
+            try:
+                self._initial_pos_timer.cancel()
+            except Exception:
+                pass
+        else:
+            now = int(self._node.get_clock().now().nanoseconds * 1e-9)
+            if now > self._initial_pos_deadline:
+                self._node.get_logger().warning("Timed out waiting for floatsam position from odom_gt")
+                try:
+                    self._initial_pos_timer.cancel()
+                except Exception:
+                    pass
+
+    def _on_goal_received(self, goal_request: dict) -> bool:
+        
+        self._node.get_logger().info(f"Goal request received: {goal_request}")
+
+        try:
+            gp : GeoPoint = GeoPoint()
+            gp.latitude = goal_request['waypoint']['latitude']
+            gp.longitude = goal_request['waypoint']['longitude']
+
+            self._goal_in_map = self._floatsam.convert_geopoint_to_map_pose_stamped(gp)
+
+            self._goal_tolerance = float(goal_request['waypoint']['tolerance'])
+            self._node.get_logger().info(f"Goal tolerance: {self._goal_tolerance}")
+
+
+            try:
+                self._goal_speed = goal_request['speed']
+                if self._goal_speed == "standard":
+                    self._goal_speed = 2.0  
+
+                elif self._goal_speed == "slow":
+                    self._goal_speed = 1.0  
+
+                elif self._goal_speed == "fast":
+                    self._goal_speed = 5.0 
+                
+                else:
+                    self._goal_speed = 2.0  
+
+            except Exception as e:
+                self._node.get_logger().warning(f"No valid speed specified, using default: {e}")
+                self._goal_speed = 2.0
+
+            
+            self._constant_speed = bool(goal_request.get('constant_speed', False))
+            self._node.get_logger().info(f"Constant speed mode: {self._constant_speed}")
+
+
+            pos = self._goal_in_map.pose.position
+            
+            self._node.get_logger().info(f"Received goal in map: [{pos.x:.2f},{pos.y:.2f},{pos.z:.2f}], tolerance: {self._goal_tolerance}, speed: {self._goal_speed}")
+            
+            return True
+        
+        except Exception as e:
+            self._node.get_logger().error(f"Failed to parse goal request: {e}")
+            traceback.print_exc()
+            return False
+
+
+    def _on_cancel_received(self) -> bool:
+        self._node.get_logger().info("Cancel requested, stopping...")
+        self._goal_in_map = None
+        return True
+
+    def _prepare_loop(self) -> None:
+        self._distance_remaining = None
+        return
+
+    def _loop_inner(self) -> bool|None:
+        if self._goal_in_map is None:
+            self._node.get_logger().info("No goal set, failing...")
+            return False
+
+        if self._goal_tolerance is None:
+            self._node.get_logger().info("No goal tolerance set, failing...")
+            return False
+
+        if self._floatsam.floatsam_in_map is None:
+            self._node.get_logger().info("No floatsam position available yet, waiting...")
+            return None
+        
+        goal_position = np.array([self._goal_in_map.pose.position.x,
+                                  self._goal_in_map.pose.position.y])
+        
+        self_position = np.array([self._floatsam.floatsam_in_map.pose.position.x,
+                                  self._floatsam.floatsam_in_map.pose.position.y])
+
+    
+        self._node.get_logger().info(f"Current position: [{self_position[0]:.2f}, {self_position[1]:.2f}]")
+        self._node.get_logger().info(f"Goal position:    [{goal_position[0]:.2f}, {goal_position[1]:.2f}]")
+        
+        goal_error = goal_position - self_position
+        goal_error_mag = np.linalg.norm(goal_error)
+        self._distance_remaining = float(goal_error_mag)
+
+        if self._distance_remaining <= self._goal_tolerance:
+            self._node.get_logger().info(f"Reached goal within tolerance {self._goal_tolerance}m")
+            return True
+        
+        if self._distance_remaining <= self._default_speed_threshold and not self._constant_speed:
+            self._desired_speed = (self._distance_remaining / self._default_speed_threshold) * self._goal_speed
+            self._node.get_logger().info(f"Slowing down, new speed: {self._desired_speed:.2f}")
+        else:
+            self._desired_speed = self._goal_speed
+    
+        self._node.get_logger().info(f"The desired speed is {self._desired_speed:.2f} m/s")
+        error_heading = float(np.arctan2(goal_error[1], goal_error[0]))
+        
+        self._node.get_logger().info(f"The distance remaining is {self._distance_remaining:.2f} m")
+        speed = float(self._desired_speed)
+        
+        rvo_request = GetSafeVelocity.Request()
+        rvo_request.robot_id = self._robot_name
+        rvo_request.pref_velocity = [speed * np.cos(error_heading), speed * np.sin(error_heading)]
+
+        move_on_place_msg = Bool()
+        move_on_place_msg.data = True 
+
+        if self._rvo_client.service_is_ready():
+            future = self._rvo_client.call_async(rvo_request)
+            deadline = time.time() + 0.5
+            while not future.done() and time.time() < deadline:
+                time.sleep(0.01)
+
+            if not future.done():
+                self._node.get_logger().warning('RVO service call timed out, skipping publish')
+                return None
+
+            rvo_response = future.result()
+            if not rvo_response.success:
+                self._node.get_logger().warning('RVO service returned success=False, skipping publish')
+                return None
+            else: 
+                self._node.get_logger().warning('RVO service returned success=Success')
+
+
+            safe_speed = rvo_response.safe_velocity[0]
+            safe_angle = rvo_response.safe_velocity[1]
+            self._node.get_logger().warning(f'safe_speed:{safe_speed}, pref_velocity:{speed}')
+            self._node.get_logger().warning(f'safe_angle:{safe_angle}, error_heading:{error_heading}')
+            if rvo_response.change == True:
+                move_on_place_msg.data = False
+        else:
+            self._node.get_logger().warning('RVO service not available, using preferred velocity directly')
+            safe_speed = speed
+            safe_angle = error_heading
+
+        yaw_msg = FloatStamped()
+        speed_msg = FloatStamped()
+        now = self._node.get_clock().now().to_msg()
+        yaw_msg.header.stamp = now
+        yaw_msg.data = safe_angle
+        speed_msg.header.stamp = now
+        speed_msg.data = safe_speed
+        self._yaw_reference_publisher.publish(yaw_msg)
+        self._speed_reference_publisher.publish(speed_msg)
+        self._move_on_place_publisher.publish(move_on_place_msg)
+
+        angle_msg = FloatStamped()
+        angle_msg.header.stamp = now
+        angle_msg.data = 0.5  
+        self._publish_captain_parametrs()
+        
+        return None
+
+    def _give_feedback(self) -> str:
+        if self._distance_remaining is not None:
+            return f"Distance remaining: {self._distance_remaining:.2f} (tolerance: {self._goal_tolerance:.2f}m)"
+        else:
+            return "No distance remaining info"
+        
+    def _publish_captain_parametrs(self):
+        """It publish the message containing the parameters for captain node"""
+        parameters = {
+            "yaw_p_gain" : self._loiter_yaw_p_gain,
+            "yaw_i_gain" : self._loiter_yaw_i_gain,
+            "yaw_d_gain" : self._loiter_yaw_d_gain,
+            "yaw_threshold" : self._loiter_yaw_threshold,
+            "yawrate_p_gain" : self._loiter_yawrate_p_gain,
+            "yawrate_i_gain" : self._loiter_yawrate_i_gain,
+            "yawrate_d_gain" : self._loiter_yaw_d_gain,
+            "velocity_p_gain" : self._loiter_velocity_p_gain, 
+            "velocity_i_gain" : self._loiter_velocity_i_gain, 
+            "velocity_d_gain" : self._loiter_velocity_d_gain
+        }
+        msg = String()
+        msg.data = json.dumps(parameters)
+        self._captain_parameters_publisher.publish(msg)
+        
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Node("floatsam_move_to_action_server")
+    node.declare_parameter('robot_name', 'floatsam_usv')
+
+    move_to_action = MoveToActionFloatSam(node)
+    executor = MultiThreadedExecutor()
+    rclpy.spin(node, executor=executor)
+    node.destroy_node()
+    rclpy.shutdown()
\ No newline at end of file
diff --git a/behaviours/floatsam/floatsam_move_to/launch/floatsam_move_to.launch.py b/behaviours/floatsam/floatsam_move_to/launch/floatsam_move_to.launch.py
new file mode 100644
index 000000000..687993a36
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/launch/floatsam_move_to.launch.py
@@ -0,0 +1,44 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    robot_name_arg = DeclareLaunchArgument(
+        'robot_name', default_value='floatsam_usv', description='Robot namespace')
+    use_sim_arg = DeclareLaunchArgument(
+        'use_sim', default_value='true', description='Use simulation')
+
+    robot_name = LaunchConfiguration('robot_name')
+    use_sim = LaunchConfiguration('use_sim')
+
+    node = Node(
+        package='floatsam_move_to',
+        executable='floatsam_move_to_action_server',
+        name='floatsam_move_to_action_server',
+        namespace=robot_name,
+        parameters=[{
+            'robot_name': robot_name,
+            'use_sim': use_sim,
+            "yaw_p_gain": 0.3,
+            "yaw_i_gain": 0.0,
+            "yaw_d_gain": 0.1,  
+            "yaw_threshold": 0.5,
+            
+            "yawrate_p_gain": 300.0,
+            "yawrate_i_gain": 0.0,
+            "yawrate_d_gain": 30.0,  
+            
+            "velocity_p_gain": 500.0,
+            "velocity_i_gain": 10.0,
+            "velocity_d_gain": 0.0
+        }],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        robot_name_arg,
+        use_sim_arg,
+        node
+    ])
diff --git a/behaviours/floatsam/floatsam_move_to/package.xml b/behaviours/floatsam/floatsam_move_to/package.xml
new file mode 100644
index 000000000..5762b5cd9
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>floatsam_move_to</name>
+  <version>0.0.0</version>
+  <description>Move to action server for FloatSam USV</description>
+  <maintainer email="lmanno@kth.se">lorenzo</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>smarc_msgs</depend>
+  <depend>floatsam_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/behaviours/floatsam/floatsam_move_to/resource/floatsam_move_to b/behaviours/floatsam/floatsam_move_to/resource/floatsam_move_to
new file mode 100644
index 000000000..e69de29bb
diff --git a/behaviours/floatsam/floatsam_move_to/setup.cfg b/behaviours/floatsam/floatsam_move_to/setup.cfg
new file mode 100644
index 000000000..e53028f7e
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/floatsam_move_to
+[install]
+install_scripts=$base/lib/floatsam_move_to
diff --git a/behaviours/floatsam/floatsam_move_to/setup.py b/behaviours/floatsam/floatsam_move_to/setup.py
new file mode 100644
index 000000000..77d85da51
--- /dev/null
+++ b/behaviours/floatsam/floatsam_move_to/setup.py
@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+import os
+import glob
+
+package_name = 'floatsam_move_to'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob.glob('launch/*.launch.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='lorenzo',
+    maintainer_email='mannolorenzo421@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'floatsam_move_to_action_server = floatsam_move_to.floatsam_move_to_server:main',
+        ],
+    },
+)