diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 19c59c15..77281efc 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/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, diff --git a/asv_setup/config/sitaw/landmark_server_params.yaml b/asv_setup/config/sitaw/landmark_server_params.yaml new file mode 100644 index 00000000..9d31f351 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml new file mode 100644 index 00000000..801738ce --- /dev/null +++ b/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 \ No newline at end of file diff --git a/asv_setup/config/sitaw/seapath_params.yaml b/asv_setup/config/sitaw/seapath_params.yaml new file mode 100644 index 00000000..fc8848fb --- /dev/null +++ b/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 diff --git a/asv_setup/launch/sim_freya.launch.py b/asv_setup/launch/sim_freya.launch.py new file mode 100644 index 00000000..6d053248 --- /dev/null +++ b/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, + ]) \ No newline at end of file diff --git a/asv_setup/launch/sitaw.launch.py b/asv_setup/launch/sitaw.launch.py new file mode 100644 index 00000000..71f796fb --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py index 0051d517..f5b1ffe4 100644 --- a/asv_setup/launch/tf.launch.py +++ b/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, ]) diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt index 0fca6d21..18909c31 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/README.md b/guidance/hybridpath_guidance/README.md index 3a512865..2c503518 100755 --- a/guidance/hybridpath_guidance/README.md +++ b/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 diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml index 72488399..7279c745 100644 --- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml +++ b/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 \ No newline at end of file + mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index 9f2a6155..39768568 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/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) \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 36255f0d..ee42402c 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -2,13 +2,20 @@ import numpy as np import rclpy from rclpy.node import Node -from geometry_msgs.msg import Pose2D +from geometry_msgs.msg import Pose2D, Point +from std_msgs.msg import Float32, Float64MultiArray, String, Bool from vortex_msgs.msg import HybridpathReference -from vortex_msgs.srv import Waypoint +from vortex_msgs.srv import Waypoint, DesiredVelocity, DesiredHeading from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler - 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, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class Guidance(Node): def __init__(self): @@ -19,84 +26,203 @@ def __init__(self): ('hybridpath_guidance.lambda_val', 0.15), ('hybridpath_guidance.path_generator_order', 1), ('hybridpath_guidance.dt', 0.1), - ('hybridpath_guidance.mu', 0.03) + ('hybridpath_guidance.mu', 0.2) ]) + # Create services, subscribers and publishers self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1) - self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) + self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback) + self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) + self.heading_publisher = self.create_publisher(Float32, 'heading', 1) + self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1) + self.heading_server = self.create_service(DesiredHeading,'heading_reference', self.heading_ref_callback) + + 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 self.dt = self.get_parameter('hybridpath_guidance.dt').get_parameter_value().double_value self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value + self.eta = np.zeros(3) - self.u_desired = 0.25 # Desired velocity + self.u_desired = 1.1 # Desired velocity + + self.use_hybridpath_heading = True + self.heading_ref = 0. + + # Initialize variables + self.waypoints = [] + self.path = None + self.s = 0. + self.w = 0. + self.v_s = 0. + self.v_ss = 0. + self.operational_mode = 'autonomous mode' + self.killswitch_active = False + + # Initialize path generator + self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) + + # Initialize signals + self.signals = HybridPathSignals() # Flags for logging self.waypoints_received = False self.waiting_message_printed = False + self.stationkeeping_flag = False + self.first_pos_flag = False + self.eta_received = False + self.initial_pos = False # Timer for guidance - timer_period = 0.1 - self.timer = self.create_timer(timer_period, self.guidance_callback) + self.timer = self.create_timer(self.dt, self.guidance_callback) + + def operation_mode_callback(self, msg: String): + self.operational_mode = msg.data + + def killswitch_callback(self, msg: Bool): + self.killswitch_active = msg.data + + def u_desired_callback(self, request, response): + self.u_desired = request.u_desired + self.get_logger().info(f"Received desired velocity: {self.u_desired}") + response.success = True + return response + + def heading_ref_callback(self, request, response): + self.heading_ref = request.heading + + self.get_logger().info(f"Received desired heading: {self.heading_ref}") + + response.success = True + + return response def waypoint_callback(self, request, response): - self.get_logger().info('Received waypoints, generating path...') - self.waypoints = request.waypoint + if self.eta_received: + self.waypoints = [Point(x=self.eta[0], y=self.eta[1])] - generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val) - self.path = generator.path + self.get_logger().info('Received waypoints, generating path...') - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + new_waypoints = request.waypoint - self.s = 0 - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) - + self.get_logger().info(f"Received {len(new_waypoints)} waypoints") + + for i, point in enumerate(new_waypoints): + + wp = [point.x, point.y] + + self.get_logger().info(f"Waypoint {i+1}: {wp}") + + self.waypoints.append(point) + + self.last_waypoint = [self.waypoints[-1].x, self.waypoints[-1].y] + + self.generator.create_path(self.waypoints) + self.path = self.generator.get_path() + + self.waypoints_received = True + self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets + self.first_pos_flag = False + + self.s = 0. + self.signals.update_path(self.path) + self.w = self.signals.get_w(self.mu, self.eta) + response.success = True + return response def eta_callback(self, msg: Odometry): + heading_msg = Float32() self.eta = self.odom_to_eta(msg) + self.heading = float(self.eta[2]) - def guidance_callback(self): - if self.waypoints_received: - self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) - signals = HybridPathSignals(self.path, self.s) - self.w = signals.get_w(self.mu, self.eta) + if not self.initial_pos: + self.eta_stationkeeping = self.eta + self.heading_ref = self.heading + self.initial_pos = True - pos = signals.get_position() - pos_der = signals.get_derivatives()[0] - pos_dder = signals.get_derivatives()[1] + heading_msg.data = self.heading + self.heading_publisher.publish(heading_msg) + self.eta_received = True - psi = signals.get_heading() - psi_der = signals.get_heading_derivative() - psi_dder = signals.get_heading_second_derivative() + 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}') - hp_msg = HybridpathReference() - hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) - hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) - hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + else: + self.get_logger().info('No eta received, cannot set stationkeeping pose') + + return response - hp_msg.w = signals.get_w(self.mu, self.eta) - hp_msg.v_s = signals.get_vs(self.u_desired) - hp_msg.v_ss = signals.get_vs_derivative(self.u_desired) + def guidance_callback(self): + if self.killswitch_active or self.operational_mode != 'autonomous mode': + return + + if not self.initial_pos: + if not self.waiting_message_printed: + self.get_logger().info('Waiting for eta') + self.waiting_message_printed = True + + return + + if self.path is None and not self.waypoints_received: + if not self.stationkeeping_flag: + self.get_logger().info(f'No waypoints received, stationkeeping at {np.round(self.eta_stationkeeping, 3)}') + self.stationkeeping_flag = True - self.guidance_publisher.publish(hp_msg) + pos = [self.eta_stationkeeping[0], self.eta_stationkeeping[1]] + pos_der = [0., 0.] + pos_dder = [0., 0.] - if self.s >= self.path.NumSubpaths: - self.waypoints_received = False - self.waiting_message_printed = False - self.get_logger().info('Last waypoint reached') + else: + self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w) + self.signals.update_s(self.s) + self.w = self.signals.get_w(self.mu, self.eta) + self.v_s = self.signals.get_vs(self.u_desired) + self.v_ss = self.signals.get_vs_derivative(self.u_desired) + + pos = self.signals.get_position() + + pos_der = self.signals.get_derivatives()[0] + pos_dder = self.signals.get_derivatives()[1] + + if self.use_hybridpath_heading and self.path is not None: + psi = self.signals.get_heading() + self.heading_ref = psi else: - if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') - self.waiting_message_printed = True + psi = self.heading_ref + + psi_der = 0.#signals.get_heading_derivative() + psi_dder = 0.#signals.get_heading_second_derivative() + + hp_msg = HybridpathReference() + hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi) + hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der) + hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder) + + hp_msg.w = self.w + hp_msg.v_s = self.v_s + hp_msg.v_ss = self.v_ss + + self.guidance_publisher.publish(hp_msg) + + if self.path is not None and self.s >= self.path.NumSubpaths: + self.waypoints_received = False + self.waiting_message_printed = False + self.stationkeeping_flag = False + self.path = None + self.eta_stationkeeping = np.array([self.last_waypoint[0], self.last_waypoint[1], self.heading_ref]) + @staticmethod def odom_to_eta(msg: Odometry) -> np.ndarray: @@ -117,9 +243,10 @@ def odom_to_eta(msg: Odometry) -> np.ndarray: ] # Convert quaternion to Euler angles - yaw = quat2euler(orientation_list)[2] + heading = quat2euler(orientation_list)[2] + + state = np.array([x, y, heading]) - state = np.array([x, y, yaw]) return state def main(args=None): diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index f27ffc20..c4bcc36e 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/config/param_joystick_interface.yaml b/mission/joystick_interface/config/param_joystick_interface.yaml index 965e3dd8..30c786f2 100755 --- a/mission/joystick_interface/config/param_joystick_interface.yaml +++ b/mission/joystick_interface/config/param_joystick_interface.yaml @@ -1,7 +1,7 @@ joystick_interface: ros__parameters: - surge_scale_factor: 100.0 - sway_scale_factor: 100.0 - yaw_scale_factor: 60.0 + surge_scale_factor: 300.0 + sway_scale_factor: 300.0 + yaw_scale_factor: 180.0 diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index 306bc82c..4aa2584d 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: @@ -79,6 +80,9 @@ def __init__(self): self.last_button_press_time_ = 0 self.debounce_duration_ = 0.25 self.state_ = States.NO_GO + self.previous_state_ = None + + self.mode_logger_done_ = False self.joystick_buttons_map_ = [] @@ -89,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) @@ -160,8 +166,12 @@ def transition_to_xbox_mode(self): """ Turns off the controller and signals that the operational mode has switched to Xbox mode. """ - self.operational_mode_signal_publisher_.publish(String(data="XBOX")) + msg = String() + msg.data = "XBOX" + self.operational_mode_signal_publisher_.publish(msg) self.state_ = States.XBOX_MODE + self.previous_state_ = States.XBOX_MODE + self.mode_logger_done_ = False def transition_to_autonomous_mode(self): """ @@ -169,8 +179,31 @@ def transition_to_autonomous_mode(self): """ wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0) self.wrench_publisher_.publish(wrench_msg) - self.operational_mode_signal_publisher_.publish(String(data="autonomous mode")) + msg = String() + msg.data = "autonomous mode" + self.operational_mode_signal_publisher_.publish(msg) self.state_ = States.AUTONOMOUS_MODE + self.previous_state_ = States.AUTONOMOUS_MODE + self.mode_logger_done_ = False + + def set_stationkeeping_pose(self): + """ + Calls a service that communicates to the guidance system to set the current + position as the stationkeeping position. + """ + 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: """ @@ -217,6 +250,7 @@ def joystick_cb(self, msg : Joy) -> Wrench: xbox_control_mode_button = buttons.get("A", 0) software_killswitch_button = buttons.get("B", 0) software_control_mode_button = buttons.get("X", 0) + software_set_home_button = buttons.get("Y", 0) left_trigger = self.left_trigger_linear_converter(axes.get('LT', 0.0)) right_trigger = self.right_trigger_linear_converter(axes.get('RT', 0.0)) @@ -229,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 @@ -240,7 +275,12 @@ def joystick_cb(self, msg : Joy) -> Wrench: # signal that killswitch is not blocking self.software_killswitch_signal_publisher_.publish( Bool(data=False)) - self.transition_to_xbox_mode() + + if self.previous_state_ == States.XBOX_MODE: + self.transition_to_xbox_mode() + + else: + self.transition_to_autonomous_mode() return else: @@ -257,18 +297,32 @@ def joystick_cb(self, msg : Joy) -> Wrench: wrench_msg = self.create_2d_wrench_message(surge, sway, yaw) if self.state_ == States.XBOX_MODE: - self.get_logger().info("XBOX mode", throttle_duration_sec=1) + if not self.mode_logger_done_: + self.get_logger().info("XBOX mode") + self.mode_logger_done_ = True + self.wrench_publisher_.publish(wrench_msg) if software_control_mode_button: self.transition_to_autonomous_mode() - if self.state_ == States.AUTONOMOUS_MODE: - self.get_logger().info("autonomous mode", throttle_duration_sec=1) + 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") + self.mode_logger_done_ = True if xbox_control_mode_button: self.transition_to_xbox_mode() + else: + if not self.mode_logger_done_: + self.get_logger().info("Killswitch is active") + self.mode_logger_done_ = True + return + return wrench_msg diff --git a/mission/landmark_server/CMakeLists.txt b/mission/landmark_server/CMakeLists.txt index 3fa085b5..74d383a1 100644 --- a/mission/landmark_server/CMakeLists.txt +++ b/mission/landmark_server/CMakeLists.txt @@ -22,24 +22,34 @@ find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) include_directories(include) +include_directories(${EIGEN3_INCLUDE_DIR}) # Create an executable add_executable(landmark_server_node src/landmark_server.cpp - src/landmark_server_main.cpp) + src/landmark_server_main.cpp + src/grid_manager.cpp) target_link_libraries( ${PROJECT_NAME}_node - pcl_common) + pcl_common + ${PCL_LIBRARIES} + Eigen3::Eigen) ament_target_dependencies(landmark_server_node rclcpp std_msgs vortex_msgs + sensor_msgs + pcl_conversions nav_msgs rclcpp_action rclcpp_components @@ -52,6 +62,8 @@ ament_target_dependencies(landmark_server_node ) install(DIRECTORY + launch + params DESTINATION share/${PROJECT_NAME}/ ) diff --git a/mission/landmark_server/include/landmark_server/grid_manager.hpp b/mission/landmark_server/include/landmark_server/grid_manager.hpp new file mode 100644 index 00000000..8bf339a5 --- /dev/null +++ b/mission/landmark_server/include/landmark_server/grid_manager.hpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include + +namespace landmark_server { + +/** + * @class GridManager + * @brief A class for managing the grid map. + */ +class GridManager { + +public: + GridManager(float resolution, uint32_t height, uint32_t width); + + ~GridManager() = default; + + /** + * @brief Update the grid with a polygon and a value to update corresponding + * cells with. + */ + void update_grid(int8_t *grid, + const Eigen::Array &vertices, + int value); + + /** + * @brief Draw a line on the grid and update the value of the cells it passes + * through. + */ + void draw_line(int x0, int y0, int x1, int y1, int8_t *grid, int value); + + /** + * @brief Fill a polygon on the grid and update the value of the cells it + * covers. + */ + void fill_polygon(int8_t *grid, + const Eigen::Array &polygon, + int value); + + /** + * @brief Check if a point is inside a polygon. + */ + bool point_in_polygon(int x, int y, + const Eigen::Array &polygon); + +private: + float resolution_; + uint32_t height_; + uint32_t width_; +}; + +} // namespace landmark_server \ No newline at end of file diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp index 799bbd7b..fbb90d2c 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server.hpp @@ -1,6 +1,10 @@ #ifndef LANDMARK_SERVER_HPP #define LANDMARK_SERVER_HPP +#include +#include +#include +#include #include #include #include @@ -15,9 +19,12 @@ #include #include +#include "nav_msgs/srv/get_map.hpp" #include #include +#include + namespace landmark_server { /** @@ -63,6 +70,11 @@ class LandmarkServerNode : public rclcpp::Node { void landmarksRecievedCallback( const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + Eigen::Array + get_convex_hull(const vortex_msgs::msg::Landmark &landmark); + + void get_grid(); + /** * @brief A shared pointer to a publisher for the LandmarkArray message type. * Publishes all landmarks currently stored in the server. @@ -76,12 +88,21 @@ class LandmarkServerNode : public rclcpp::Node { */ rclcpp::Publisher::SharedPtr posePublisher_; + rclcpp::Publisher::SharedPtr gridPublisher_; + + rclcpp::Publisher::SharedPtr + convex_hull_publisher_; + rclcpp::Publisher::SharedPtr + cluster_publisher_; + /** * @brief A shared pointer to a LandmarkArray message. * The array contains all landmarks currently stored by the server. */ std::shared_ptr storedLandmarks_; + nav_msgs::msg::OccupancyGrid grid_msg_; + /** * @brief A shared pointer to an rclcpp_action server for handling filtered * landmarks. @@ -89,6 +110,10 @@ class LandmarkServerNode : public rclcpp::Node { rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Client::SharedPtr grid_client_; + + std::unique_ptr grid_manager_ = nullptr; + /** * @brief Handles the goal request for the `handle_goal` function. * diff --git a/mission/landmark_server/launch/landmark_server_launch.py b/mission/landmark_server/launch/landmark_server_launch.py new file mode 100644 index 00000000..72a1d29a --- /dev/null +++ b/mission/landmark_server/launch/landmark_server_launch.py @@ -0,0 +1,26 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import (DeclareLaunchArgument) +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + default_params_file = os.path.join(get_package_share_directory('landmark_server'),'params','landmark_server_params.yaml') + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + landmark_server_node = Node( + package='landmark_server', + executable='landmark_server_node', + name='landmark_server_node', + parameters=[params_file], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + params_file_arg, + landmark_server_node + ]) \ No newline at end of file diff --git a/mission/landmark_server/package.xml b/mission/landmark_server/package.xml index 65fe7f54..0247154e 100644 --- a/mission/landmark_server/package.xml +++ b/mission/landmark_server/package.xml @@ -18,6 +18,9 @@ nav_msgs tf2 tf2_geometry_msgs + sensor_msgs + pcl_conversions + diff --git a/mission/landmark_server/params/landmark_server_params.yaml b/mission/landmark_server/params/landmark_server_params.yaml new file mode 100644 index 00000000..c95a6384 --- /dev/null +++ b/mission/landmark_server/params/landmark_server_params.yaml @@ -0,0 +1,6 @@ +landmark_server_node: + ros__parameters: + fixed_frame: world + target_frame: base_link + wall_width: 0.1 + use_grid: false \ No newline at end of file diff --git a/mission/landmark_server/src/grid_manager.cpp b/mission/landmark_server/src/grid_manager.cpp new file mode 100644 index 00000000..b8df5637 --- /dev/null +++ b/mission/landmark_server/src/grid_manager.cpp @@ -0,0 +1,97 @@ +#include +#include + +namespace landmark_server { + +GridManager::GridManager(float resolution, uint32_t height, uint32_t width) + : resolution_(resolution), height_(height), width_(width) {} + +void GridManager::update_grid( + int8_t *grid, const Eigen::Array &vertices, + int value) { + // Convert to grid coordinates + for (Eigen::Index i = 0; i < vertices.cols(); ++i) { + Eigen::Index j = (i + 1) % vertices.cols(); + int x0 = static_cast(vertices(0, i) / resolution_ + width_ / 2); + int y0 = static_cast(vertices(1, i) / resolution_ + height_ / 2); + int x1 = static_cast(vertices(0, j) / resolution_ + width_ / 2); + int y1 = static_cast(vertices(1, j) / resolution_ + height_ / 2); + + draw_line(x0, y0, x1, y1, grid, value); + } + if (vertices.cols() > 1) { + fill_polygon(grid, vertices, value); + } +} + +void GridManager::fill_polygon( + int8_t *grid, const Eigen::Array &polygon, + int value) { + double max_x = polygon.row(0).maxCoeff(); + double min_x = polygon.row(0).minCoeff(); + double max_y = polygon.row(1).maxCoeff(); + double min_y = polygon.row(1).minCoeff(); + + for (int x = min_x; x < max_x; x++) { + for (int y = min_y; y < max_y; y++) { + if (x >= 0 && x < static_cast(width_) && y >= 0 && + y < static_cast(height_) && point_in_polygon(x, y, polygon)) { + grid[y * width_ + x] += value; + } + } + } +} + +bool GridManager::point_in_polygon( + int x, int y, const Eigen::Array &polygon) { + int i, j; + bool c = false; + for (i = 0, j = polygon.cols() - 1; i < polygon.cols(); j = i++) { + if (((polygon(1, i) > y) != (polygon(1, j) > y)) && + (x < (polygon(0, j) - polygon(0, i)) * (y - polygon(1, i)) / + (polygon(1, j) - polygon(1, i)) + + polygon(0, i))) + c = !c; + } + return c; +} + +void GridManager::draw_line(int x0, int y0, int x1, int y1, int8_t *grid, + int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? height_ : width_; + int ybounds = steep ? width_ : height_; + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid[y + x * height_] += value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid[y * width_ + x] += value; + } + } + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; + } + } +} + +} // namespace landmark_server \ No newline at end of file diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp index 2a8875b8..7300126c 100644 --- a/mission/landmark_server/src/landmark_server.cpp +++ b/mission/landmark_server/src/landmark_server.cpp @@ -1,4 +1,4 @@ -#include "landmark_server/landmark_server.hpp" +#include using std::placeholders::_1, std::placeholders::_2; using LandmarkArray = vortex_msgs::msg::LandmarkArray; @@ -12,26 +12,54 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) // Define the quality of service profile for publisher and subscriber rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), - qos_profile); + auto qos_best_effort = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + // Set QoS profile + rmw_qos_profile_t qos_profile_sub = rmw_qos_profile_default; + qos_profile_sub.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos_profile_sub.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile_sub.depth = + 10; // You can adjust this depth as needed for your use case + + auto qos_reliable = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_sub.history, qos_profile_sub.depth), + qos_profile_sub); storedLandmarks_ = std::make_shared(); landmark_sub_ = this->create_subscription( - "target_tracking/landmarks", qos, + "target_tracking/landmarks", qos_reliable, std::bind(&LandmarkServerNode::landmarksRecievedCallback, this, _1)); landmarkPublisher_ = - this->create_publisher("landmarks_out", qos); + this->create_publisher("landmarks_out", qos_best_effort); posePublisher_ = this->create_publisher( - "landmark_poses_out", qos); + "landmark_poses_out", qos_best_effort); + + gridPublisher_ = this->create_publisher( + "grid", qos_best_effort); + + rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data; + qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + auto qos_sensor = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor.history, 1), + qos_profile_sensor); + + convex_hull_publisher_ = + this->create_publisher("landmark/hull", + qos_sensor); + cluster_publisher_ = this->create_publisher( + "landmark/cluster", qos_sensor); tf2_buffer_ = std::make_shared(this->get_clock()); tf2_listener_ = std::make_shared(*tf2_buffer_); + declare_parameter("fixed_frame", "world"); declare_parameter("target_frame", "base_link"); - declare_parameter("world_frame", "world_frame"); + declare_parameter("wall_width", 0.1); + declare_parameter("use_grid", "false"); // Create the act. action_server_ = rclcpp_action::create_server( @@ -39,6 +67,108 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options) std::bind(&LandmarkServerNode::handle_goal, this, _1, _2), std::bind(&LandmarkServerNode::handle_cancel, this, _1), std::bind(&LandmarkServerNode::handle_accepted, this, _1)); + + grid_client_ = create_client("get_map"); + + std::thread(&LandmarkServerNode::get_grid, this).detach(); +} + +Eigen::Array LandmarkServerNode::get_convex_hull( + const vortex_msgs::msg::Landmark &landmark) { + if (landmark.landmark_type == vortex_msgs::msg::Landmark::WALL) { + Eigen::Array hull(2, 4); + float x1 = landmark.shape.polygon.points[0].x; + float y1 = landmark.shape.polygon.points[0].y; + float x2 = landmark.shape.polygon.points[1].x; + float y2 = landmark.shape.polygon.points[1].y; + + // Calculate the direction vector + float dx = x2 - x1; + float dy = y2 - y1; + + // Normalize the direction vector + float length = std::sqrt(dx * dx + dy * dy); + dx /= length; + dy /= length; + + // Perpendicular vector with the given width + float wall_width = get_parameter("wall_width").as_double(); + float wx = -dy * wall_width / 2; // half the width to each side + float wy = dx * wall_width / 2; + + // Calculate the four points of the rectangle in clockwise order + hull(0, 0) = x1 + wx; + hull(1, 0) = y1 + wy; + hull(0, 1) = x2 + wx; + hull(1, 1) = y2 + wy; + hull(0, 2) = x2 - wx; + hull(1, 2) = y2 - wy; + hull(0, 3) = x1 - wx; + hull(1, 3) = y1 - wy; + return hull; + } + pcl::PointCloud cluster; + cluster.resize(landmark.shape.polygon.points.size()); + for (size_t i = 0; i < landmark.shape.polygon.points.size(); i++) { + cluster.points[i].x = landmark.shape.polygon.points[i].x; + cluster.points[i].y = landmark.shape.polygon.points[i].y; + cluster.points[i].z = 1.0; + } + // Compute the convex hull of the cluster + pcl::PointCloud convex_hull; + pcl::ConvexHull chull; + chull.setDimension(2); + chull.setInputCloud(cluster.makeShared()); + chull.reconstruct(convex_hull); + + Eigen::Array hull(2, convex_hull.size()); + for (size_t i = 0; i < convex_hull.size(); i++) { + hull(0, i) = convex_hull.points[i].x; + hull(1, i) = convex_hull.points[i].y; + } + return hull; +} + +void LandmarkServerNode::get_grid() { + while (true) { + rclcpp::sleep_for(std::chrono::seconds(1)); + if (!grid_client_->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(this->get_logger(), "Service not available after waiting"); + continue; + } + + auto request = std::make_shared(); + auto result_future = grid_client_->async_send_request(request); + + // Wait for the result within a specified timeout period + auto status = result_future.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::ready) { + try { + auto result = result_future.get(); + if (result->map.data.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Received empty map from grid client"); + continue; + } + grid_msg_ = result->map; + + grid_manager_ = std::make_unique(grid_msg_.info.resolution, + grid_msg_.info.height, + grid_msg_.info.width); + RCLCPP_INFO(this->get_logger(), + "Successfully received map from grid client"); + return; + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), + "Exception while getting result from future: %s", + e.what()); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to get map from grid client within timeout period"); + continue; + } + } } void LandmarkServerNode::landmarksRecievedCallback( @@ -49,39 +179,62 @@ void LandmarkServerNode::landmarksRecievedCallback( return; } - for (const auto &landmark : msg->landmarks) { - - if (landmark.action == 0) { - // Remove landmarks with matching id and landmark_type - storedLandmarks_->landmarks.erase( - std::remove_if(storedLandmarks_->landmarks.begin(), - storedLandmarks_->landmarks.end(), - [&](const auto &storedLandmark) { - return storedLandmark.id == landmark.id && - storedLandmark.landmark_type == - landmark.landmark_type; - }), - storedLandmarks_->landmarks.end()); - } else if (landmark.action == 1) { - // Find the landmark if it already exists - auto it = std::find_if( - storedLandmarks_->landmarks.begin(), - storedLandmarks_->landmarks.end(), [&](const auto &storedLandmark) { - return storedLandmark.landmark_type == landmark.landmark_type && - storedLandmark.id == landmark.id; - }); + if (this->get_parameter("use_grid").as_bool() && grid_manager_ == nullptr) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Grid manager not initialized"); + return; + } + for (const auto &landmark : msg->landmarks) { + auto it = std::find_if( + storedLandmarks_->landmarks.begin(), storedLandmarks_->landmarks.end(), + [&](const auto &storedLandmark) { + return storedLandmark.landmark_type == landmark.landmark_type && + storedLandmark.id == landmark.id; + }); + + if (landmark.action == vortex_msgs::msg::Landmark::ADD_ACTION) { if (it != storedLandmarks_->landmarks.end()) { - // Update the existing landmark - *it = landmark; + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Requested to add already existing landmark"); } else { // Add the new landmark + + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), 200); + } storedLandmarks_->landmarks.push_back(landmark); } + continue; + } + if (it == storedLandmarks_->landmarks.end()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Requested to remove or update non-existing landmark"); + continue; + } + + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), -200); + } + + if (landmark.action == vortex_msgs::msg::Landmark::REMOVE_ACTION) { + storedLandmarks_->landmarks.erase(it); + } else if (landmark.action == vortex_msgs::msg::Landmark::UPDATE_ACTION) { + if (this->get_parameter("use_grid").as_bool()) { + grid_manager_->update_grid(grid_msg_.data.data(), + get_convex_hull(landmark), 200); + } + *it = landmark; } } + grid_msg_.header.stamp = rclcpp::Clock().now(); // Publish the landmarks + gridPublisher_->publish(grid_msg_); landmarkPublisher_->publish(*storedLandmarks_); posePublisher_->publish(poseArrayCreater(*storedLandmarks_)); } @@ -94,8 +247,7 @@ geometry_msgs::msg::PoseArray LandmarkServerNode::poseArrayCreater( if (!landmarks.landmarks.empty()) { poseArray.header.frame_id = landmarks.landmarks.at(0).odom.header.frame_id; } else { - poseArray.header.frame_id = get_parameter("world_frame").as_string(); - poseArray.header.stamp = rclcpp::Clock().now(); + poseArray.header.frame_id = get_parameter("fixed_frame").as_string(); } // Timestamps for stored landmarks may vary so use current time for // visualization @@ -199,31 +351,29 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks( goal_handle) { const auto goal = goal_handle->get_goal(); _Float32 distance = goal->distance; + uint8_t filter_type = goal->landmark_types; vortex_msgs::msg::OdometryArray filteredLandmarksOdoms; for (const auto &landmark : storedLandmarks_->landmarks) { - if (goal->landmark_types.empty() && distance == 0.0) { + // filter_type 0 returns all landmarks + if (filter_type == 0 && distance == 0.0) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } else if (goal->landmark_types.empty()) { + } else if (filter_type == 0) { if (calculateDistance(landmark.odom.pose.pose.position, landmark.odom.header) <= distance) { filteredLandmarksOdoms.odoms.push_back(landmark.odom); } } else if (distance == 0.0) { - for (const auto &type : goal->landmark_types) { - if (landmark.landmark_type == type) { - filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } + if (landmark.landmark_type == filter_type) { + filteredLandmarksOdoms.odoms.push_back(landmark.odom); } } else { - for (const auto &type : goal->landmark_types) { - if (landmark.landmark_type == type && - calculateDistance(landmark.odom.pose.pose.position, - landmark.odom.header) <= distance) { - filteredLandmarksOdoms.odoms.push_back(landmark.odom); - } + if (landmark.landmark_type == filter_type && + calculateDistance(landmark.odom.pose.pose.position, + landmark.odom.header) <= distance) { + filteredLandmarksOdoms.odoms.push_back(landmark.odom); } } } @@ -237,13 +387,14 @@ void LandmarkServerNode::requestLogger( const auto goal = goal_handle->get_goal(); double distance = goal->distance; - if (distance == 0.0 && goal->landmark_types.empty()) { + if (distance == 0.0 && + goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { RCLCPP_INFO_STREAM(this->get_logger(), "Received request to return all landmarks."); return; } - if (goal->landmark_types.empty()) { + if (goal->landmark_types == vortex_msgs::msg::Landmark::NONE) { RCLCPP_INFO_STREAM( this->get_logger(), "Received request to return all landmarks within distance " << distance @@ -253,12 +404,14 @@ void LandmarkServerNode::requestLogger( std::stringstream types_log; types_log << "Received request to return landmarks by type filter: ["; - for (auto it = goal->landmark_types.begin(); it != goal->landmark_types.end(); - it++) { - types_log << *it; - if (std::next(it) != goal->landmark_types.end()) { - types_log << ", "; - } + if (goal->landmark_types & vortex_msgs::msg::Landmark::BUOY) { + types_log << "BUOY, "; + } + if (goal->landmark_types & vortex_msgs::msg::Landmark::BOAT) { + types_log << "BOAT, "; + } + if (goal->landmark_types & vortex_msgs::msg::Landmark::WALL) { + types_log << "WALL, "; } types_log << "]."; RCLCPP_INFO_STREAM(this->get_logger(), types_log.str()); @@ -274,8 +427,8 @@ LandmarkServerNode::calculateDistance(const geometry_msgs::msg::Point &point, try { // Lookup the transformation geometry_msgs::msg::TransformStamped transform_stamped = - tf2_buffer_->lookupTransform("base_link", header.frame_id, header.stamp, - rclcpp::Duration(1, 0)); + tf2_buffer_->lookupTransform(target_frame, header.frame_id, + header.stamp, rclcpp::Duration(1, 0)); // Transform the point cloud geometry_msgs::msg::Point transformed_point; diff --git a/mission/map_manager/CMakeLists.txt b/mission/map_manager/CMakeLists.txt new file mode 100644 index 00000000..f76c00e3 --- /dev/null +++ b/mission/map_manager/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(map_manager) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +include_directories(include) + +add_executable(map_manager_node src/map_manager_ros.cpp + src/map_manager_node.cpp +) + +target_link_libraries( ${PROJECT_NAME}_node + pcl_common) + +ament_target_dependencies( + map_manager_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros +) + +install( + DIRECTORY include/ + DESTINATION include + +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}/ + ) + + install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/mission/map_manager/include/map_manager/map_manager_ros.hpp b/mission/map_manager/include/map_manager/map_manager_ros.hpp new file mode 100644 index 00000000..41797e1c --- /dev/null +++ b/mission/map_manager/include/map_manager/map_manager_ros.hpp @@ -0,0 +1,93 @@ +#ifndef MAP_MANAGER_HPP +#define MAP_MANAGER_HPP + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/srv/get_map.hpp" +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace map_manager { + +/** + * @class MapManagerNode + * @brief A class representing a node for handling maps in a ROS 2 system. + * + * This class inherits from rclcpp::Node and provides functionality for + * receiving map messages and storing them in a map. + */ +class MapManagerNode : public rclcpp::Node { +public: + /** + * @brief Constructor for the MapManagerNode class. + * + * @param options The options for configuring the node. + */ + explicit MapManagerNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for the MapManagerNode class. + */ + ~MapManagerNode(){}; + +private: + constexpr double deg2rad(double degrees) const; + std::array lla2flat(double lat, double lon) const; + std::array flat2lla(double px, double py) const; + /** + * @brief Publishes static transform from world NED to world SEU to use for + * foxglove visualization. + * + */ + void publish_foxglove_vis_frame(const rclcpp::Time &time) const; + + void publish_map_to_odom_tf(double map_lat, double map_lon, + const rclcpp::Time &time) const; + + geometry_msgs::msg::PolygonStamped + processCoordinates(const std::vector> &coordinates); + geometry_msgs::msg::PolygonStamped + readPolygonFromFile(const std::string &filename); + void mapOriginCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + nav_msgs::msg::OccupancyGrid createOccupancyGrid(); + geometry_msgs::msg::Pose calculate_map_origin(); + void insert_landmask(nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon); + void handle_get_map_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + void fillOutsidePolygon(nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon); + + rclcpp::Subscription::SharedPtr odom_origin_sub_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr + landmask_pub_; + rclcpp::Service::SharedPtr grid_service_; + rclcpp::Publisher::SharedPtr map_origin_pub_; + bool map_origin_set_ = false; + bool use_predef_landmask_; + double map_origin_lat_; + double map_origin_lon_; + std::string landmask_file_; + std::string grid_frame_id_; + std::shared_ptr static_tf_broadcaster_; +}; + +} // namespace map_manager + +#endif // MAP_MANAGER_HPP \ No newline at end of file diff --git a/mission/map_manager/launch/map_manager_launch.py b/mission/map_manager/launch/map_manager_launch.py new file mode 100644 index 00000000..d7ee6dfa --- /dev/null +++ b/mission/map_manager/launch/map_manager_launch.py @@ -0,0 +1,26 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import (DeclareLaunchArgument) +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + default_params_file = os.path.join(get_package_share_directory('map_manager'),'params','map_manager_params.yaml') + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + map_manager_node = Node( + package='map_manager', + executable='map_manager_node', + name='map_manager_node', + parameters=[params_file], + # arguments=['--ros-args', '--log-level', 'DEBUG'], + output='screen', + ) + return LaunchDescription([ + params_file_arg, + map_manager_node + ]) \ No newline at end of file diff --git a/mission/map_manager/package.xml b/mission/map_manager/package.xml new file mode 100644 index 00000000..cee4d527 --- /dev/null +++ b/mission/map_manager/package.xml @@ -0,0 +1,26 @@ + + + + map_manager + 0.0.0 + TODO: Package description + jorgen + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2_ros + tf2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mission/map_manager/params/map_manager_params.yaml b/mission/map_manager/params/map_manager_params.yaml new file mode 100644 index 00000000..559e5a3e --- /dev/null +++ b/mission/map_manager/params/map_manager_params.yaml @@ -0,0 +1,10 @@ +map_manager_node: + ros__parameters: + use_predef_landmask: false + map_resolution: 0.2 + map_width: 1000 + map_height: 1000 + frame_id: "world" + map_origin_lat: 0.0 + map_origin_lon: 0.0 + use_predef_map_origin: false diff --git a/mission/map_manager/src/map_manager_node.cpp b/mission/map_manager/src/map_manager_node.cpp new file mode 100644 index 00000000..420dca0c --- /dev/null +++ b/mission/map_manager/src/map_manager_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/mission/map_manager/src/map_manager_ros.cpp b/mission/map_manager/src/map_manager_ros.cpp new file mode 100644 index 00000000..1552209c --- /dev/null +++ b/mission/map_manager/src/map_manager_ros.cpp @@ -0,0 +1,400 @@ +#include + +namespace map_manager { + +MapManagerNode::MapManagerNode(const rclcpp::NodeOptions &options) + : Node("map_manager_node", options) { + + rmw_qos_profile_t qos_profile_transient_local = rmw_qos_profile_parameters; + qos_profile_transient_local.durability = + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + auto qos_transient_local = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile_transient_local.history, 1), + qos_profile_transient_local); + + declare_parameter("use_predef_landmask", false); + declare_parameter( + "landmask_file", + "src/vortex-asv/mission/map_manager/params/land_polygon.yaml"); + declare_parameter("map_resolution", 0.2); + declare_parameter("map_width", 1000); + declare_parameter("map_height", 1000); + declare_parameter("frame_id", "map"); + declare_parameter("map_origin_lat", 0.0); + declare_parameter("map_origin_lon", 0.0); + declare_parameter("use_predef_map_origin", false); + + landmask_file_ = get_parameter("landmask_file").as_string(); + landmask_pub_ = this->create_publisher( + "landmask", qos_transient_local); + map_pub_ = this->create_publisher( + "map", qos_transient_local); + + map_origin_pub_ = this->create_publisher( + "/map/origin", qos_transient_local); + + if (this->get_parameter("use_predef_map_origin").as_bool()) { + map_origin_lat_ = this->get_parameter("map_origin_lat").as_double(); + map_origin_lon_ = this->get_parameter("map_origin_lon").as_double(); + map_origin_set_ = true; + + auto grid = createOccupancyGrid(); + if (this->get_parameter("use_predef_landmask").as_bool()) { + auto polygon = readPolygonFromFile(landmask_file_); + landmask_pub_->publish(polygon); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + } + + map_pub_->publish(grid); + } else { + odom_origin_sub_ = this->create_subscription( + "/oodm/origin", qos_transient_local, + std::bind(&MapManagerNode::mapOriginCallback, this, + std::placeholders::_1)); + } + + grid_service_ = this->create_service( + "get_map", + std::bind(&MapManagerNode::handle_get_map_request, // Callback function + this, // Pointer to the object + std::placeholders::_1, // Placeholder for request header + std::placeholders::_2, // Placeholder for request + std::placeholders::_3 // Placeholder for response + )); +} + +void MapManagerNode::mapOriginCallback( + const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + double odom_origin_lat = msg->latitude; + double odom_origin_lon = msg->longitude; + map_origin_set_ = true; + odom_origin_sub_ = nullptr; + publish_map_to_odom_tf(odom_origin_lat, odom_origin_lon, msg->header.stamp); + publish_foxglove_vis_frame(msg->header.stamp); + if (this->get_parameter("use_predef_landmask").as_bool()) { + landmask_pub_->publish(readPolygonFromFile(landmask_file_)); + } +} + +void MapManagerNode::handle_get_map_request( + [[maybe_unused]] const std::shared_ptr request_header, + [[maybe_unused]] const std::shared_ptr + request, + const std::shared_ptr response) { + if (!map_origin_set_) { + RCLCPP_WARN(this->get_logger(), "Map origin not set, cannot provide map"); + return; + } + RCLCPP_INFO(this->get_logger(), "Received request for map"); + + nav_msgs::msg::OccupancyGrid grid = createOccupancyGrid(); + if (this->get_parameter("use_predef_landmask").as_bool()) { + auto polygon = readPolygonFromFile(landmask_file_); + fillOutsidePolygon(grid, polygon); + insert_landmask(grid, polygon); + } + response->map = grid; + RCLCPP_INFO(this->get_logger(), "Map sent"); +} + +constexpr double MapManagerNode::deg2rad(double degrees) const { + return degrees * M_PI / 180.0; +} + +std::array MapManagerNode::lla2flat(double lat, double lon) const { + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + const double psi_rad = + 0.0; // Angular direction of the flat Earth x-axis, specified as a scalar. + // The angular direction is the degrees clockwise from north, + // which is the angle in degrees used for converting flat Earth x and + // y coordinates to the north and east coordinates + + // Convert angles from degrees to radians + const double lat_rad = deg2rad(lat); + const double lon_rad = deg2rad(lon); + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); + + // Calculate delta latitude and delta longitude in radians + const double dlat = lat_rad - origin_lat_rad; + const double dlon = lon_rad - origin_lon_rad; + + // Radius of curvature in the vertical prime (RN) + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Radius of curvature in the meridian (RM) + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Changes in the north (dN) and east (dE) positions + const double dN = RM * dlat; + const double dE = RN * cos(origin_lat_rad) * dlon; + + // Transformation from North-East to flat x-y coordinates + const double px = cos(psi_rad) * dN - sin(psi_rad) * dE; + const double py = sin(psi_rad) * dN + cos(psi_rad) * dE; + + return {px, py}; +} + +void MapManagerNode::publish_map_to_odom_tf(double map_lat, double map_lon, + const rclcpp::Time &time) const { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "odom"; + + auto [x, y] = lla2flat(map_lat, map_lon); + + transformStamped.transform.translation.x = -x; + transformStamped.transform.translation.y = -y; + transformStamped.transform.translation.z = 0; + + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 0.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 1.0; + + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); +} + +void MapManagerNode::publish_foxglove_vis_frame( + const rclcpp::Time &time) const { + // Setup the transform + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = time; + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = "map_visualization"; + + transformStamped.transform.translation.x = 0.0; + transformStamped.transform.translation.y = 0.0; + transformStamped.transform.translation.z = 0.0; + // NED to SEU + transformStamped.transform.rotation.x = 0.0; + transformStamped.transform.rotation.y = 1.0; + transformStamped.transform.rotation.z = 0.0; + transformStamped.transform.rotation.w = 0.0; + + // Broadcast the static transform + static_tf_broadcaster_->sendTransform(transformStamped); +} + +std::array MapManagerNode::flat2lla(double px, double py) const { + // Earth constants + const double R = 6378137.0; // WGS-84 Earth semimajor axis (meters) + const double f = 1.0 / 298.257223563; // Flattening of the earth + + // Convert origin from degrees to radians + const double origin_lat_rad = deg2rad(map_origin_lat_); + const double origin_lon_rad = deg2rad(map_origin_lon_); + + // Radius of curvature in the vertical prime (RN) and in the meridian (RM) + const double RN = + R / sqrt(1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + const double RM = RN * (1.0 - (2.0 * f - f * f)) / + (1.0 - (2.0 * f - f * f) * pow(sin(origin_lat_rad), 2)); + + // Calculate changes in latitude and longitude + double dN = px; // Change in north direction + double dE = py; // Change in east direction + double dlat = dN / RM; + double dlon = dE / (RN * cos(origin_lat_rad)); + + // Convert delta lat and lon from radians back to degrees + double new_lat = (origin_lat_rad + dlat) * 180.0 / M_PI; + double new_lon = (origin_lon_rad + dlon) * 180.0 / M_PI; + + return {new_lat, new_lon}; +} + +geometry_msgs::msg::PolygonStamped +MapManagerNode::readPolygonFromFile(const std::string &filename) { + std::ifstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + filename); + } + + std::string line; + std::vector> coords; + while (std::getline(file, line)) { + std::stringstream ss(line); + double lat, lon; + if (ss >> lat >> lon) { + coords.push_back({lat, lon}); + } else { + std::cerr << "Failed to parse line: " << line << std::endl; + } + } + + file.close(); + std::cout << "coords size: " << coords.size() << "\n"; + + return processCoordinates(coords); +} + +geometry_msgs::msg::PolygonStamped MapManagerNode::processCoordinates( + const std::vector> &coordinates) { + geometry_msgs::msg::PolygonStamped polygon; + polygon.header.frame_id = get_parameter("frame_id").as_string(); + polygon.header.stamp = this->now(); + std::cout << "coords size: " << coordinates.size() << "\n"; + for (const auto &coord : coordinates) { + std::array flat = lla2flat(coord[0], coord[1]); + geometry_msgs::msg::Point32 point; + point.x = flat[0]; + point.y = flat[1]; + point.z = 0; + polygon.polygon.points.push_back(point); + } + std::cout << "Polygon size: " << polygon.polygon.points.size() << std::endl; + return polygon; +} + +nav_msgs::msg::OccupancyGrid MapManagerNode::createOccupancyGrid() { + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = get_parameter("frame_id").as_string(); + grid.header.stamp = this->now(); + grid.info.resolution = get_parameter("map_resolution").as_double(); + grid.info.width = get_parameter("map_width").as_int(); + grid.info.height = get_parameter("map_height").as_int(); + grid.info.origin = calculate_map_origin(); + grid.info.map_load_time = this->now(); + // initialize grid with zeros + grid.data.resize(grid.info.width * grid.info.height, 0); + return grid; +} + +geometry_msgs::msg::Pose MapManagerNode::calculate_map_origin() { + + // Map centre is (0,0) in map frame, origin is bottom left corner + double half_width_meters = -(get_parameter("map_width").as_int() * + get_parameter("map_resolution").as_double()) / + 2.0; + double half_height_meters = -(get_parameter("map_height").as_int() * + get_parameter("map_resolution").as_double()) / + 2.0; + geometry_msgs::msg::Pose map_origin; + map_origin.position.x = half_width_meters; + map_origin.position.y = half_height_meters; + map_origin.position.z = 0.0; + map_origin.orientation.x = 0.0; + map_origin.orientation.y = 0.0; + map_origin.orientation.z = 0.0; + map_origin.orientation.w = 1.0; + return map_origin; +} + +// Helper function to draw line using Bresenham's algorithm +void drawLine(int x0, int y0, int x1, int y1, + nav_msgs::msg::OccupancyGrid &grid, int value) { + bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); + if (steep) { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) { + std::swap(x0, x1); + std::swap(y0, y1); + } + int dx = x1 - x0; + int dy = std::abs(y1 - y0); + int error = 2 * dy - dx; + int ystep = (y0 < y1) ? 1 : -1; + int xbounds = steep ? grid.info.height : grid.info.width; + int ybounds = steep ? grid.info.width : grid.info.height; + int y = y0; + + for (int x = x0; x <= x1; x++) { + if (steep) { + if (x >= 0 && x < xbounds && y >= 0 && y < ybounds) { + grid.data[y + x * grid.info.width] = value; + } + } else { + if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { + grid.data[y * grid.info.width + x] = value; + } + } + if (error > 0) { + y += ystep; + error -= 2 * (dx - dy); + } else { + error += 2 * dy; + } + } +} + +bool isPointInsidePolygon(int px, int py, + const geometry_msgs::msg::PolygonStamped &polygon, + const nav_msgs::msg::OccupancyGrid &grid) { + int count = 0; + int n = polygon.polygon.points.size(); + for (int i = 0; i < n; i++) { + const geometry_msgs::msg::Point32 &start = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32 &end = + polygon.polygon.points[(i + 1) % n]; + int x0 = start.x / grid.info.resolution + grid.info.width / 2; + int y0 = start.y / grid.info.resolution + grid.info.height / 2; + int x1 = end.x / grid.info.resolution + grid.info.width / 2; + int y1 = end.y / grid.info.resolution + grid.info.height / 2; + + // Check if the line from start to end intersects with the line from (px, + // py) to (infinity, py) + if ((y0 > py) != + (y1 > py)) { // If the point is between the y bounds of the segment + // Calculate x coordinate of the intersection point of the line segment + // with the horizontal line through py + float intersectX = + (y1 - y0 != 0) ? (x1 - x0) * (float)(py - y0) / (float)(y1 - y0) + x0 + : x0; + if (px < intersectX) // Ray crossing from the left + count++; + } + } + return (count % 2) == 1; // Odd inside, even outside +} + +void MapManagerNode::fillOutsidePolygon( + nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon) { + int outside_value = 50; // Set occupancy value for outside (0-100) + + // Iterate over each pixel in the grid + for (size_t x = 0; x < grid.info.width; x++) { + for (size_t y = 0; y < grid.info.height; y++) { + if (!isPointInsidePolygon(x, y, polygon, grid)) { + grid.data[y * grid.info.width + x] = outside_value; + } + } + } +} + +void MapManagerNode::insert_landmask( + nav_msgs::msg::OccupancyGrid &grid, + const geometry_msgs::msg::PolygonStamped &polygon) { + int value = 100; // Set this to the desired occupancy value for land (0-100) + for (uint i = 0; i < polygon.polygon.points.size(); i++) { + + const geometry_msgs::msg::Point32 ¤t = polygon.polygon.points[i]; + const geometry_msgs::msg::Point32 &next = + polygon.polygon + .points[(i + 1) % polygon.polygon.points.size()]; // Loop back to + // the first point + // Convert map xy to grid indices + int x0 = current.x / grid.info.resolution + grid.info.width / 2; + int y0 = current.y / grid.info.resolution + grid.info.height / 2; + int x1 = next.x / grid.info.resolution + grid.info.width / 2; + int y1 = next.y / grid.info.resolution + grid.info.height / 2; + + // Draw line on the grid + drawLine(x0, y0, x1, y1, grid, value); + } +} + +} // namespace map_manager \ No newline at end of file diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index b5dde0d1..018e9192 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -12,7 +12,7 @@ rclpy geometry_msgs ros2launch - rnav_msgs + nav_msgs vortex_msgs ament_lint_auto diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml index e7ac823a..d8a91934 100644 --- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml +++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: hybridpath_controller: - K1_diag: [10.0, 10.0, 10.0] # First gain matrix - K2_diag: [60.0, 60.0, 60.0] # Second gain matrix \ No newline at end of file + K1_diag: [1., 1., 5.] # First gain matrix + K2_diag: [40.0, 30.0, 40.0] # Second gain matrix \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index ff95a51c..891aa677 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -4,7 +4,13 @@ from transforms3d.euler import quat2euler class AdaptiveBackstep: - def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: + def __init__(self): + self.K_1 = np.eye(3) + self.K_2 = np.eye(3) + self.M = np.eye(3) + self.D = np.eye(3) + + def update_parameters(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None: self.K_1 = K1 self.K_2 = K2 self.M = M @@ -27,6 +33,7 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda # Extract values from the state and reference eta = state[:3] + # eta[0] = 0. nu = state[3:] w = reference.w v_s = reference.v_s @@ -54,8 +61,34 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda tau = -self.K_2 @ z2 + self.calculate_coriolis_matrix(nu) + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w) + self.eta_error = eta_error + self.z1 = z1 + self.alpha1 = alpha1 + self.z2 = z2 + self.ds_alpha1 = ds_alpha1 + self.sigma1 = sigma1 + return tau + def get_eta_error(self): + return self.eta_error + + def get_z1(self): + return self.z1 + + def get_alpha1(self): + return self.alpha1 + + def get_z2(self): + return self.z2 + + def get_sigma1(self): + return self.sigma1 + + def get_ds_alpha1(self): + return self.ds_alpha1 + + @staticmethod def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: """ @@ -66,7 +99,7 @@ def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: [0, 0, 5.5], [82.5, -5.5, 0] ]) - return C_A @ nu + return (C_A @ nu) @staticmethod def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray: @@ -118,6 +151,8 @@ def odom_to_state(msg: Odometry) -> np.ndarray: # Convert quaternion to Euler angles yaw = quat2euler(orientation_list)[2] + # yaw = np.deg2rad(yaw) + u = msg.twist.twist.linear.x v = msg.twist.twist.linear.y r = msg.twist.twist.angular.z diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index d74f1abb..21262565 100755 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py @@ -7,6 +7,13 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from vortex_msgs.msg import HybridpathReference +from std_msgs.msg import Float64MultiArray, String, Bool +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy +from rcl_interfaces.msg import SetParametersResult + + +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class HybridPathControllerNode(Node): def __init__(self): @@ -20,31 +27,72 @@ def __init__(self): ('physical.damping_matrix_diag', [10.0, 10.0, 5.0]) ]) - self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.state_callback, 1) + self.parameters_updated = False + + self.killswitch_active = False + self.operational_mode = 'autonomous mode' + + self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile) self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1) self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1) + 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) + + # Debug publishers + self.eta_error_publisher = self.create_publisher(Float64MultiArray, 'eta_error', 10) + self.z1_publisher = self.create_publisher(Float64MultiArray, 'z1', 10) + self.alpha1_publisher = self.create_publisher(Float64MultiArray, 'alpha1', 10) + self.z2_publisher = self.create_publisher(Float64MultiArray, 'z2', 10) + self.ds_alpha1_publisher = self.create_publisher(Float64MultiArray, 'ds_alpha1', 10) + self.sigma1_publisher = self.create_publisher(Float64MultiArray, 'sigma1', 10) + self.tau_publisher = self.create_publisher(Float64MultiArray, 'tau', 10) + + self.AB_controller_ = AdaptiveBackstep() + + self.update_controller_parameters() + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.add_on_set_parameters_callback(self.parameter_callback) + + self.get_logger().info("hybridpath_controller_node started") + + def operation_mode_callback(self, msg: String): + self.operational_mode = msg.data + + def killswitch_callback(self, msg: Bool): + self.killswitch_active = msg.data + + def update_controller_parameters(self): - # Get parameters K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value K2_diag = self.get_parameter('hybridpath_controller.K2_diag').get_parameter_value().double_array_value D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - # Transform parameters to diagonal matrices K1 = np.diag(K1_diag) K2 = np.diag(K2_diag) D = np.diag(D_diag) - - # Reshape M to a 3x3 matrix M = np.reshape(M, (3, 3)) - # Create controller object - self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D) + self.AB_controller_.update_parameters(K1, K2, M, D) - controller_period = 0.1 - self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + self.get_logger().info("Updated controller parameters") - self.get_logger().info("hybridpath_controller_node started") + def parameter_callback(self, params): + self.parameters_updated = True + for param in params: + if param.name == 'hybridpath_controller.K1_diag': + self.get_logger().info(f"Updated K1_diag to {param.value}") + elif param.name == 'hybridpath_controller.K2_diag': + self.get_logger().info(f"Updated K2_diag to {param.value}") + elif param.name == 'physical.damping_matrix_diag': + self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") + elif param.name == 'physical.inertia_matrix': + self.get_logger().info(f"Updated inertia_matrix to {param.value}") + + # self.update_controller_parameters() + return SetParametersResult(successful=True) def state_callback(self, msg: Odometry): """ @@ -59,6 +107,13 @@ def controller_callback(self): """ Callback function for the controller timer. This function calculates the control input and publishes the control input. """ + if self.killswitch_active or self.operational_mode != 'autonomous mode': + return + + if self.parameters_updated: + self.update_controller_parameters() + self.parameters_updated = False + if hasattr(self, 'state_odom') and hasattr(self, 'reference'): control_input = self.AB_controller_.control_law(self.state_odom, self.reference) wrench_msg = Wrench() @@ -67,6 +122,36 @@ def controller_callback(self): wrench_msg.torque.z = control_input[2] self.wrench_publisher_.publish(wrench_msg) + # Debug publishers + eta_error_msg = Float64MultiArray() + eta_error_msg.data = self.AB_controller_.get_eta_error().tolist() + self.eta_error_publisher.publish(eta_error_msg) + + z1_msg = Float64MultiArray() + z1_msg.data = self.AB_controller_.get_z1().tolist() + self.z1_publisher.publish(z1_msg) + + alpha1_msg = Float64MultiArray() + alpha1_msg.data = self.AB_controller_.get_alpha1().tolist() + self.alpha1_publisher.publish(alpha1_msg) + + z2_msg = Float64MultiArray() + z2_msg.data = self.AB_controller_.get_z2().tolist() + self.z2_publisher.publish(z2_msg) + + ds_alpha1_msg = Float64MultiArray() + ds_alpha1_msg.data = self.AB_controller_.get_ds_alpha1().tolist() + self.ds_alpha1_publisher.publish(ds_alpha1_msg) + + sigma1_msg = Float64MultiArray() + sigma1_msg.data = self.AB_controller_.get_sigma1().tolist() + self.sigma1_publisher.publish(sigma1_msg) + + tau_msg = Float64MultiArray() + tau_msg.data = control_input.tolist() + self.tau_publisher.publish(tau_msg) + + def main(args=None): rclpy.init(args=args) node = HybridPathControllerNode() diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv index bac8cf21..39c47657 100644 --- a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv +++ b/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv @@ -82,20 +82,20 @@ -2964.98 1459.55 -2782.91 1464.04 -2522.36 1468.54 --2112.45 1473.03 --1955.09 1477.53 --1551.28 1482.02 --1327.8 1486.52 --913.3 1491.01 --506.29 1495.51 +-0.06 1473.03 +-0.05 1477.53 +-0.04 1482.02 +-0.03 1486.52 +-0.02 1491.01 +-0.01 1495.51 0 1500 -614.46 1504.04 -967.25 1508.08 -1501.92 1512.12 -1965.98 1516.16 -2315.18 1520.2 -2789.34 1524.24 -3218.91 1528.28 +0.01 1504.04 +0.02 1508.08 +0.03 1512.12 +0.04 1516.16 +0.05 1520.2 +0.06 1524.24 +0.07 1528.28 3442.74 1532.32 3763.95 1536.36 4118.76 1540.4