Added ability to set stationkeeping pose by pressing Y

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/hybridpath_guidance/hybridpath_guidance_node.py

@@ -11,6 +11,7 @@
 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
 import threading
+from std_srvs.srv import Empty
 
 
 qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
@@ -40,6 +41,8 @@ def __init__(self):
         self.operational_mode_subscriber = self.create_subscription(String, 'softWareOperationMode', self.operation_mode_callback, 10)
         self.killswitch_subscriber = self.create_subscription(Bool, 'softWareKillSwitch', self.killswitch_callback, 10)
 
+        self.set_stationkeeping_pose_service = self.create_service(Empty, 'set_stationkeeping_pose', self.set_stationkeeping_pose_callback)
+
         # Get parameters
         self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value
         self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value
@@ -98,6 +101,7 @@ def yaw_ref_callback(self, request, response):
         self.get_logger().info(f"Received desired heading: {self.yaw_ref}")
 
         response.success = True
+
         return response
 
     def waypoint_callback(self, request, response):
@@ -150,6 +154,16 @@ def eta_callback(self, msg: Odometry):
         self.yaw_publisher.publish(yaw_msg)
         self.eta_received = True
 
+    def set_stationkeeping_pose_callback(self, request, response):
+        if self.eta_received:
+            self.eta_stationkeeping = self.eta
+            self.get_logger().info(f'Set stationkeeping pose to {self.eta_stationkeeping}')
+
+        else:
+            self.get_logger().info('No eta received, cannot set stationkeeping pose')
+
+        return response
+
     def guidance_callback(self):
         with self.lock:
             if self.killswitch_active or self.operational_mode != 'autonomous mode':

mission/joystick_interface/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(ament_cmake_python REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 
 ament_python_install_package(joystick_interface)
 

mission/joystick_interface/joystick_interface/joystick_interface.py

@@ -5,6 +5,7 @@
 from sensor_msgs.msg import Joy
 from std_msgs.msg import Bool
 from std_msgs.msg import String
+from std_srvs.srv import Empty
 
 
 class States:
@@ -92,6 +93,8 @@ def __init__(self):
         self.wrench_publisher_ = self.create_publisher(Wrench,
                                                       "thrust/wrench_input",
                                                       1)
+
+        self.set_stationkeeping_pose_client = self.create_client(Empty, 'set_stationkeeping_pose')
 
         self.declare_parameter('surge_scale_factor', 50.0)
         self.declare_parameter('sway_scale_factor', 50.0)
@@ -183,12 +186,24 @@ def transition_to_autonomous_mode(self):
         self.previous_state_ = States.AUTONOMOUS_MODE
         self.mode_logger_done_ = False
 
-    def set_home_position(self):
+    def set_stationkeeping_pose(self):
         """
         Calls a service that communicates to the guidance system to set the current
-        position as the home position.
+        position as the stationkeeping position.
         """
-        pass
+        request = Empty.Request()
+        future = self.set_stationkeeping_pose_client.call_async(request)
+        future.add_done_callback(self.set_stationkeeping_pose_callback)
+
+    def set_stationkeeping_pose_callback(self, future):
+        """
+        Callback function for the set_stationkeeping_pose service.
+        """
+        try:
+            response = future.result()
+        except Exception as e:
+            self.get_logger().info(
+                f"Service call failed {str(e)}")
 
     def joystick_cb(self, msg : Joy) -> Wrench:
         """
@@ -248,9 +263,10 @@ def joystick_cb(self, msg : Joy) -> Wrench:
             software_control_mode_button = False
             xbox_control_mode_button = False
             software_killswitch_button = False
+            software_set_home_button = False
 
         # If any button is pressed, update the last button press time
-        if software_control_mode_button or xbox_control_mode_button or software_killswitch_button:
+        if software_control_mode_button or xbox_control_mode_button or software_killswitch_button or software_set_home_button:
             self.last_button_press_time_ = current_time
 
         # Toggle ks on and off
@@ -290,6 +306,9 @@ def joystick_cb(self, msg : Joy) -> Wrench:
             if software_control_mode_button:
                 self.transition_to_autonomous_mode()
 
+            if software_set_home_button:
+                self.set_stationkeeping_pose()
+
         elif self.state_ == States.AUTONOMOUS_MODE:
             if not self.mode_logger_done_:
                 self.get_logger().info("autonomous mode")