diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index 8b4fd5a..c85f4ee 100644 --- a/guidance/hybridpath_guidance/CMakeLists.txt +++ b/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}) diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index cd0df95..e068607 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/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': diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index f27ffc2..c4bcc36 100755 --- a/mission/joystick_interface/CMakeLists.txt +++ b/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) diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index bc8c52b..4aa2584 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/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")