diff --git a/guidance/dp_guidance/CMakeLists.txt b/guidance/dp_guidance/CMakeLists.txt deleted file mode 100644 index 7cc58d78..00000000 --- a/guidance/dp_guidance/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(dp_guidance) - -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_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - dp_guidance/dp_guidance_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/guidance/dp_guidance/README.md b/guidance/dp_guidance/README.md deleted file mode 100755 index 3dbdad76..00000000 --- a/guidance/dp_guidance/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Dynamic Positioning (DP) Guidance - -This package provides the implementation of DP guidance for the Vortex ASV. - -## Usage - -To use the DP guidance launch it using: `ros2 launch dp_guidance dp_guidance.launch` - -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}]"` - -## Configuration - -You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. \ No newline at end of file diff --git a/guidance/dp_guidance/config/dp_guidance_config.yaml b/guidance/dp_guidance/config/dp_guidance_config.yaml deleted file mode 100644 index 02db4e76..00000000 --- a/guidance/dp_guidance/config/dp_guidance_config.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - dp_guidance: - dt: 0.1 # Time step \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/__init__.py b/guidance/dp_guidance/dp_guidance/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/guidance/dp_guidance/dp_guidance/conversions.py b/guidance/dp_guidance/dp_guidance/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/guidance/dp_guidance/dp_guidance/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py deleted file mode 100755 index 1ddbf8cf..00000000 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Pose2D -from vortex_msgs.srv import Waypoint -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -from std_msgs.msg import Float32 -from conversions import odometrymsg_to_state, state_to_odometrymsg -from reference_filter import ReferenceFilter -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class Guidance(Node): - def __init__(self): - super().__init__("dp_guidance") - self.declare_parameters( - namespace='', - parameters=[ - ('dp_guidance.dt', 0.1) - ]) - - self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback) - self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) - self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1) - self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1) - self.yaw_ref_publisher = self.create_publisher(Float32, 'yaw_ref', 1) - - # Get parameters - self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value - - # Flags for logging - self.waypoints_received = False - self.waiting_message_printed = False - - self.init_pos = False - self.eta_received = False - self.eta_logger = True - - self.yaw_ref = 0. - - self.xd = np.zeros(9) - - self.reference_filter = ReferenceFilter() - - # Timer for guidance - timer_period = 0.1 - self.timer = self.create_timer(timer_period, self.guidance_callback) - - def waypoint_callback(self, request, response): - self.init_pos = False - self.waypoints = request.waypoint - self.get_logger().info(f'Received waypoints: {self.waypoints}') - self.xd = np.zeros(9) - self.waypoints_received = True - self.waiting_message_printed = False # Reset this flag to handle multiple waypoint sets - response.success = True - return response - - def eta_callback(self, msg: Odometry): - self.eta = odometrymsg_to_state(msg)[:3] - self.eta_received = True - self.yaw_publisher.publish(Float32(data=self.eta[2])) - self.yaw_ref_publisher.publish(Float32(data=self.yaw_ref)) - - def guidance_callback(self): - if self.waypoints_received and self.eta_received: - if not self.init_pos: - self.xd[0:3] = self.eta - self.get_logger().info(f"Reference initialized at {self.xd[0:3]}") - self.init_pos = True - last_waypoint = self.waypoints[-1] - self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.yaw_ref]) - x_next = self.reference_filter.step(self.eta_ref, self.xd) - self.xd = x_next - # self.get_logger().info(f'x_next[0]: {x_next[0]}') - # self.get_logger().info(f'x_next[0]: {x_next[1]}') - # self.get_logger().info(f'x_next[0]: {x_next[2]}') - - odom_msg = Odometry() - odom_msg = state_to_odometrymsg(x_next[:3]) - # odom_msg = state_to_odometrymsg(self.eta_ref) - self.guidance_publisher.publish(odom_msg) - - else: - if not self.eta_received and self.eta_logger: - self.get_logger().info("Waiting for eta") - self.eta_logger = False - - if not self.waiting_message_printed: - self.get_logger().info('Waiting for waypoints to be received') - self.waiting_message_printed = True - -def main(args=None): - rclpy.init(args=args) - guidance = Guidance() - rclpy.spin(guidance) - guidance.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py deleted file mode 100644 index 45b243f3..00000000 --- a/guidance/dp_guidance/dp_guidance/reference_filter.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import control - -class ReferenceFilter: - def __init__(self): - zeta = 0.8 - omega_b = 0.05 - omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2)) - - I = np.eye(3) - Omega = 2*zeta*omega_n*I - # Gamma = omega_n**2*I - Delta = zeta*I - Ad = np.zeros((9,9)) - Ad[0:3,3:6] = I - Ad[3:6,6:9] = I - Ad[6:9,0:3] = -Omega**3 - Ad[6:9,3:6] = -(2*Delta+I)@Omega**2 - Ad[6:9,6:9] = -(2*Delta+I)@Omega - - Bd = np.zeros((9,3)) - Bd[6:9,:] = Omega**3 - - sys = control.ss(Ad, Bd, np.zeros((9,9)), np.zeros((9,3))) - sysd = control.c2d(sys, 0.1) - - self.Ad = sysd.A - self.Bd = sysd.B - - def step(self, r, xd): - x_next = self.Ad@xd + self.Bd@r - return x_next - - def get_eta(self, xd): - return xd[:,0:3] - - def get_nu(self, xd): - nu = np.zeros((len(xd),3)) - for i in range(len(xd)): - psi = xd[i,2] - nu[i,:] = (self.rotationMatrix(psi).transpose())@xd[i,3:6] - return nu - - @staticmethod - def rotationMatrix(psi): - R = np.array([[np.cos(psi), -np.sin(psi), 0], - [np.sin(psi), np.cos(psi), 0], - [0, 0, 1]]) - return R diff --git a/guidance/dp_guidance/launch/dp_guidance.launch.py b/guidance/dp_guidance/launch/dp_guidance.launch.py deleted file mode 100644 index 1f9ad872..00000000 --- a/guidance/dp_guidance/launch/dp_guidance.launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - dp_guidance_node = Node( - package='dp_guidance', - executable='dp_guidance_node.py', - name='dp_guidance_node', - parameters=[os.path.join(get_package_share_directory('dp_guidance'),'config','dp_guidance_config.yaml')], - output='screen', - ) - return LaunchDescription([ - dp_guidance_node - ]) - diff --git a/guidance/dp_guidance/package.xml b/guidance/dp_guidance/package.xml deleted file mode 100644 index 33547bcb..00000000 --- a/guidance/dp_guidance/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - dp_guidance - 0.0.0 - This package provides the implementation of hybrid path guidance for the Vortex ASV. - vortex - MIT - - rclpy - nav_msgs - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - ament_cmake - - diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py deleted file mode 100755 index 5d4395d5..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import Waypoint -from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -import numpy as np -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class OdomPublisher(Node): - def __init__(self): - super().__init__('odom_publisher') - - self.eta_odom_publisher = self.create_publisher(Odometry, '/seapath/odom/ned', qos_profile) - - timer_period = 0.01 - self.timer = self.create_timer(timer_period, self.odom_callback) - - self.get_logger().info("Odom publisher started") - - def odom_callback(self): - - noise = np.random.normal(0, 0.005, 3) - - msg = Odometry() - msg.pose.pose.position.x = 0.0 + noise[0] - msg.pose.pose.position.y = 0.0 + noise[1] - msg.pose.pose.position.z = 0.0 - msg.pose.pose.orientation.x = 0.0 - msg.pose.pose.orientation.y = 0.0 - msg.pose.pose.orientation.z = 0.0 - msg.pose.pose.orientation.w = 1.0 - msg.twist.twist.linear.x = 0.0 + noise[0] - msg.twist.twist.linear.y = 0.0 + noise[1] - msg.twist.twist.linear.z = 0.0 - msg.twist.twist.angular.x = 0.0 - msg.twist.twist.angular.y = 0.0 - msg.twist.twist.angular.z = 0.0 + noise[2] - - self.eta_odom_publisher.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = OdomPublisher() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py deleted file mode 100644 index 033afd22..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py +++ /dev/null @@ -1,48 +0,0 @@ -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import DesiredVelocity - -class DesiredVelocityClient(Node): - def __init__(self): - super().__init__('desired_velocity_client') - self.client = self.create_client(DesiredVelocity, 'u_desired') - - while not self.client.wait_for_service(timeout_sec=2.0): - self.get_logger().info('service not available, waiting again...') - - self.send_request() - - def send_request(self): - req = DesiredVelocity.Request() - req.u = 0.3 - self.get_logger().info(f'Sending request: {req}') - self.future = self.client.call_async(req) - self.future.add_done_callback(self.get_response) - - def get_response(self, future): - try: - response = future.result() - self.get_logger().info(f'Received response: {response}') - if response.success: - self.destroy_node() - rclpy.shutdown() - - except Exception as e: - self.get_logger().error('Service call failed %r' % (e,)) - -def main(args=None): - rclpy.init(args=args) - - client = DesiredVelocityClient() - - try: - rclpy.spin(client) - except Exception as e: - client.get_logger().error('Error in DesiredVelocityClient: %r' % (e,)) - finally: - if rclpy.ok(): - client.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py deleted file mode 100755 index 5be5c27e..00000000 --- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import Waypoint -from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -import numpy as np -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class WaypointClient(Node): - def __init__(self): - super().__init__('waypoint_client') - self.eta_odom = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile) - - self.eta_received = False - - while not self.eta_received: - self.get_logger().info('Waiting for eta...') - rclpy.spin_once(self) - - self.client = self.create_client(Waypoint, 'waypoint_list') - - while not self.client.wait_for_service(timeout_sec=2.0): - self.get_logger().info('service not available, waiting again...') - - self.send_request() - - def eta_callback(self, msg: Odometry): - self.eta = self.odom_to_eta(msg) - #self.get_logger().info(f'Received eta {self.eta}') - self.eta_received = True - - def send_request(self): - if self.eta_received: - req = Waypoint.Request() - # wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]] - # wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]] - wp_list = [[self.eta[0], self.eta[1] + 40.]] - req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list] - self.get_logger().info(f'Sending request: {req}') - self.future = self.client.call_async(req) - self.future.add_done_callback(self.get_response) - - def get_response(self, future): - try: - response = future.result() - self.get_logger().info(f'Received response: {response}') - if response.success: - self.destroy_node() - rclpy.shutdown() - - except Exception as e: - self.get_logger().error('Service call failed %r' % (e,)) - - @staticmethod - def odom_to_eta(msg: Odometry) -> np.ndarray: - """ - Converts an Odometry message to 3DOF eta vector. - - Args: - msg (Odometry): The Odometry message to convert. - - Returns: - np.ndarray: The eta vector. - """ - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles - yaw = quat2euler(orientation_list)[2] - - state = np.array([x, y, yaw]) - return state - -def main(args=None): - rclpy.init(args=args) - waypoint_client_node = WaypointClient() - try: - rclpy.spin(waypoint_client_node) - except Exception as e: - waypoint_client_node.get_logger().error(f'Unhandled exception: {e}') - finally: - if rclpy.ok(): - waypoint_client_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt b/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt deleted file mode 100644 index aed2ab9e..00000000 --- a/mission/njord_tasks/collision_avoidance_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(collision_avoidance_task) - -# 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(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(collision_avoidance_task_node - src/collision_avoidance_task_node.cpp - src/collision_avoidance_task_ros.cpp) - -target_link_libraries(collision_avoidance_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(collision_avoidance_task_node - rclcpp - njord_task_base - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions -) - -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/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp deleted file mode 100644 index c6309cab..00000000 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef COLLISION_AVOIDANCE_TASK_ROS_HPP -#define COLLISION_AVOIDANCE_TASK_ROS_HPP - -#include -#include -#include -#include -#include -#include -#include -#include // Required for tf2::doTransform - -namespace collision_avoidance_task { - -struct LandmarkOdomID { - nav_msgs::msg::Odometry odom; - int32_t id; -}; -class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { -public: - explicit CollisionAvoidanceTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~CollisionAvoidanceTaskNode(){}; - - /** - * @brief Main task for the CollisionAvoidanceTaskNode class. - */ - void main_task(); - - /** - * @brief Predict the positions of the first two buoys - * - * @return Eigen::Array predicted_positions - */ - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - void track_vessel(const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair); - - nav_msgs::msg::Odometry get_vessel_odom(); - - LandmarkOdomID - filter_landmarks(const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair); - - void vessel_collision_heading(); - - double calculate_angle(const geometry_msgs::msg::Vector3 &twist1, - const geometry_msgs::msg::Vector3 &twist2); - - bool - has_vessel_passed_freya(const Eigen::Vector2d &direction_vector_downwards); - - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green); - -private: - mutable std::mutex vessel_odom_mutex_; - bool new_vessel_odom_msg_ = false; - nav_msgs::msg::Odometry vessel_odom_; - std::condition_variable vessel_odom_cv_; -}; - -} // namespace collision_avoidance_task - -#endif // COLLISION_AVOIDANCE_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py b/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py deleted file mode 100644 index 40e7e2fa..00000000 --- a/mission/njord_tasks/collision_avoidance_task/launch/collision_avoidance_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - collision_avoidance_task_node = Node( - package='collision_avoidance_task', - executable='collision_avoidance_task_node', - name='collision_avoidance_task_node', - parameters=[os.path.join(get_package_share_directory('collision_avoidance_task'),'params','collision_avoidance_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - collision_avoidance_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/package.xml b/mission/njord_tasks/collision_avoidance_task/package.xml deleted file mode 100644 index 9e2ea608..00000000 --- a/mission/njord_tasks/collision_avoidance_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - collision_avoidance_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml b/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml deleted file mode 100644 index db084e8f..00000000 --- a/mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -collision_avoidance_task_node: - ros__parameters: - map_origin_lat: -33.72213988382845 - map_origin_lon: 150.67413500672993 - map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 0.05243377001522731 - gps_end_y: 37.196639612524166 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. - distance_between_buoy_pairs: 5.0 # Distance between buoy pairs in meters - vessel_assignment_confidence: 5 # Number of consequtive identical assignments from landmark to vessel before we consider the assignment as correct \ No newline at end of file diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp deleted file mode 100644 index 9cf7ead4..00000000 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_node.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#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/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp deleted file mode 100644 index 9c79855c..00000000 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ /dev/null @@ -1,729 +0,0 @@ -#include - -namespace collision_avoidance_task { - -CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( - const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("collision_avoidance_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("distance_between_buoy_pairs", 5.0); - declare_parameter("vessel_assignment_confidence", 5); - - std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); -} - -void CollisionAvoidanceTaskNode::main_task() { - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Collision avoidance task started"); - odom_start_point_ = get_odom()->pose.pose.position; - Eigen::Array predicted_first_buoy_pair = - predict_first_buoy_pair(); - - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - // First first buoy pair is far away, should be closer before trying to - // measure it. - double gps_end_x = this->get_parameter("gps_end_x").as_double(); - double gps_end_y = this->get_parameter("gps_end_y").as_double(); - Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - odom_start_point_.x, - gps_end_y - odom_start_point_.y; - direction_vector_to_end.normalize(); - - double distance = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance > 5.0) { - auto odom = get_odom(); - geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance - 3); - waypoint_toward_start.z = 0.0; - send_waypoint(waypoint_toward_start); - set_desired_heading(odom_start_point_, waypoint_toward_start); - reach_waypoint(3.0); - } - - std::vector measured_first_buoy_pair = - get_buoy_landmarks(predicted_first_buoy_pair); - if (measured_first_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_first_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_first_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(3.0); - - // Second buoy pair - Eigen::Array predicted_second_buoy_pair = - predict_second_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_second_buoy_pair(0, 0); - buoy_red_0.y = predicted_second_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_second_buoy_pair(0, 1); - buoy_green_1.y = predicted_second_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_second_buoy_pair = - get_buoy_landmarks(predicted_second_buoy_pair); - if (measured_second_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_second_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_second_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_second_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_second_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (measured_second_buoy_pair[0].pose_odom_frame.position.x + - measured_second_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (measured_second_buoy_pair[0].pose_odom_frame.position.y + - measured_second_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(odom_start_point_, waypoint_second_pair); - reach_waypoint(3.0); - - Eigen::Vector2d direction_vector_forwards; - Eigen::Vector2d direction_vector_downwards; - direction_vector_forwards << waypoint_second_pair.x - waypoint_first_pair.x, - waypoint_second_pair.y - waypoint_first_pair.y; - direction_vector_forwards.normalize(); - direction_vector_downwards - << (measured_first_buoy_pair[1].pose_odom_frame.position.x + - measured_second_buoy_pair[1].pose_odom_frame.position.x) / - 2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_second_buoy_pair[0].pose_odom_frame.position.x) / - 2, - (measured_first_buoy_pair[1].pose_odom_frame.position.y + - measured_second_buoy_pair[1].pose_odom_frame.position.y) / - 2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_second_buoy_pair[0].pose_odom_frame.position.y) / - 2; - direction_vector_downwards.normalize(); - - geometry_msgs::msg::Point waypoint_midpoint; - waypoint_midpoint.x = - waypoint_second_pair.x + direction_vector_forwards(0) * 10; - waypoint_midpoint.y = - waypoint_second_pair.y + direction_vector_forwards(1) * 10; - waypoint_midpoint.z = 0.0; - send_waypoint(waypoint_midpoint); - set_desired_heading(waypoint_second_pair, waypoint_midpoint); - - std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, - direction_vector_forwards, direction_vector_downwards, - waypoint_second_pair)) - .detach(); - RCLCPP_INFO(this->get_logger(), "Tracking vessel"); - reach_waypoint(9.0); - // Run until the angle between the twist vectors are between 60 and 120 - // degrees - vessel_collision_heading(); - auto vessel_odom = get_vessel_odom(); - auto freya_odom = get_odom(); - - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel - << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, - vessel_odom.pose.pose.position.y - freya_odom->pose.pose.position.y; - // Project the vector do find "height" difference of vessels - double downwards_diff = - direction_vector_freya_to_vessel.dot(direction_vector_downwards); - geometry_msgs::msg::Point first_avoidance_waypoint; - if (std::abs(downwards_diff) > 5) { - first_avoidance_waypoint.x = - vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; - first_avoidance_waypoint.y = - vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; - first_avoidance_waypoint.z = 0.0; - send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, - first_avoidance_waypoint); - } else { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - - direction_vector_forwards(0) * 3 + - direction_vector_downwards(0) * 2; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - - direction_vector_forwards(1) * 3 + - direction_vector_downwards(1) * 2; - first_avoidance_waypoint.z = 0.0; - send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, - first_avoidance_waypoint); - } - - while (!has_vessel_passed_freya(direction_vector_downwards)) { - RCLCPP_INFO(this->get_logger(), "Vessel has not passed Freya yet"); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - freya_odom = get_odom(); - geometry_msgs::msg::Point second_avoidance_waypoint; - second_avoidance_waypoint.x = - freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; - second_avoidance_waypoint.y = - freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; - second_avoidance_waypoint.z = 0.0; - - // reach_waypoint(1.0); - - geometry_msgs::msg::Point back_on_track_waypoint; - back_on_track_waypoint.x = waypoint_second_pair.x + - direction_vector_forwards(0) * 15 + - direction_vector_downwards(0) * 0; - back_on_track_waypoint.y = waypoint_second_pair.y + - direction_vector_forwards(1) * 15 + - direction_vector_downwards(1) * 0; - back_on_track_waypoint.z = 0.0; - - auto request = std::make_shared(); - request->waypoint.push_back(second_avoidance_waypoint); - request->waypoint.push_back(back_on_track_waypoint); - auto result_future = waypoint_client_->async_send_request(request); - - // Check if the service was successful - auto odom = get_odom(); - double dx = back_on_track_waypoint.x - odom->pose.pose.position.x; - double dy = back_on_track_waypoint.y - odom->pose.pose.position.y; - double desired_heading = std::atan2(dy, dx); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, desired_heading); - - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = back_on_track_waypoint.x; - waypoint_vis.pose.position.y = back_on_track_waypoint.y; - waypoint_vis.pose.orientation = tf2::toMsg(q); - waypoint_visualization_pub_->publish(waypoint_vis); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - - previous_waypoint_odom_frame_ = back_on_track_waypoint; - - set_desired_heading(second_avoidance_waypoint, back_on_track_waypoint); - reach_waypoint(3.0); - - Eigen::Array predicted_third_buoy_pair = - predict_third_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_third_buoy_pair(0, 0); - buoy_red_0.y = predicted_third_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_third_buoy_pair(0, 1); - buoy_green_1.y = predicted_third_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_third_buoy_pair = - get_buoy_landmarks(predicted_third_buoy_pair); - if (measured_third_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_third_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_third_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_third_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_third_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = - (measured_third_buoy_pair[0].pose_odom_frame.position.x + - measured_third_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_third_pair.y = - (measured_third_buoy_pair[0].pose_odom_frame.position.y + - measured_third_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_third_pair.z = 0.0; - send_waypoint(waypoint_third_pair); - set_desired_heading(back_on_track_waypoint, waypoint_third_pair); - reach_waypoint(3.0); - - Eigen::Array predicted_fourth_buoy_pair = - predict_fourth_buoy_pair( - measured_third_buoy_pair[0].pose_odom_frame.position, - measured_third_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_fourth_buoy_pair(0, 0); - buoy_red_0.y = predicted_fourth_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_fourth_buoy_pair(0, 1); - buoy_green_1.y = predicted_fourth_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_fourth_buoy_pair = - get_buoy_landmarks(predicted_fourth_buoy_pair); - if (measured_fourth_buoy_pair.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_fourth_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_fourth_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_fourth_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_fourth_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fourth_pair; - waypoint_fourth_pair.x = - (measured_fourth_buoy_pair[0].pose_odom_frame.position.x + - measured_fourth_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_fourth_pair.y = - (measured_fourth_buoy_pair[0].pose_odom_frame.position.y + - measured_fourth_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_fourth_pair.z = 0.0; - send_waypoint(waypoint_fourth_pair); - set_desired_heading(waypoint_third_pair, waypoint_fourth_pair); - reach_waypoint(3.0); - - geometry_msgs::msg::Point waypoint_end; - waypoint_end.x = this->get_parameter("gps_end_x").as_double(); - waypoint_end.y = this->get_parameter("gps_end_y").as_double(); - waypoint_end.z = 0.0; - send_waypoint(waypoint_end); - set_desired_heading(waypoint_fourth_pair, waypoint_end); -} - -Eigen::Array -CollisionAvoidanceTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - double distance_between_buoy_pairs = - this->get_parameter("distance_between_buoy_pairs").as_double(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 0) = - buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; - predicted_positions(0, 1) = - buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 1) = - buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; - - return predicted_positions; -} - -void CollisionAvoidanceTaskNode::track_vessel( - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { - std::shared_ptr landmark_msg; - int32_t prev_vessel_id = -1; - int vessel_assignment_confidence = - this->get_parameter("vessel_assignment_confidence").as_int(); - int current_confidence = 0; - while (true) { - landmark_msg = get_landmarks_odom_frame(); - LandmarkOdomID vessel = - filter_landmarks(*landmark_msg, direction_vector_downwards, - direction_vector_forwards, waypoint_second_buoy_pair); - - if (vessel.id == -1) { - current_confidence = 0; - continue; - } - if (current_confidence == 0) { - prev_vessel_id = vessel.id; - current_confidence++; - continue; - } - if (vessel.id != prev_vessel_id) { - current_confidence = 0; - continue; - } - if (vessel.id == prev_vessel_id) { - if (current_confidence < vessel_assignment_confidence) { - current_confidence++; - continue; - } - current_confidence++; - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_ = vessel.odom; - new_vessel_odom_msg_ = true; - lock.unlock(); - vessel_odom_cv_.notify_one(); - continue; - } - } -} - -LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( - const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d &direction_vector_downwards, - const Eigen::Vector2d &direction_vector_forwards, - const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { - double max_velocity = std::numeric_limits::min(); - LandmarkOdomID filtered_landmark; - filtered_landmark.id = -1; - for (auto landmark : landmarks.landmarks) { - double velocity = std::hypot(landmark.odom.twist.twist.linear.x, - landmark.odom.twist.twist.linear.y); - geometry_msgs::msg::Point landmark_pose = landmark.odom.pose.pose.position; - Eigen::Vector2d vector_waypoint_to_landmark; - vector_waypoint_to_landmark - << landmark_pose.x - waypoint_second_buoy_pair.x, - landmark_pose.y - waypoint_second_buoy_pair.y; - double projection_down = - vector_waypoint_to_landmark.dot(direction_vector_downwards); - double projection_forwards = - vector_waypoint_to_landmark.dot(direction_vector_forwards); - - if (projection_down < -1 || projection_down > 20) { - continue; - } - if (projection_forwards < -1 || projection_forwards > 20) { - continue; - } - if (velocity > max_velocity && velocity > 0.1) { - max_velocity = velocity; - filtered_landmark.odom = landmark.odom; - filtered_landmark.id = landmark.id; - } - } - return filtered_landmark; -} - -nav_msgs::msg::Odometry CollisionAvoidanceTaskNode::get_vessel_odom() { - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_cv_.wait(lock, [this] { return new_vessel_odom_msg_; }); - new_vessel_odom_msg_ = false; - lock.unlock(); - return vessel_odom_; -} - -void CollisionAvoidanceTaskNode::vessel_collision_heading() { - RCLCPP_INFO(this->get_logger(), "Checking vessel collision heading"); - while (true) { - auto freya_odom = get_odom(); - bool transform_success = false; - geometry_msgs::msg::TransformStamped transform; - while (!transform_success) { - try { - transform = tf_buffer_->lookupTransform("odom", "base_link", - tf2::TimePointZero); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - geometry_msgs::msg::Vector3 freya_odom_twist; - tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, - transform); - auto vessel_odom = get_vessel_odom(); - double collision_angle = - calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); - if (collision_angle > 60 || collision_angle < 120) { - return; - } - continue; - } -} - -// Function to calculate the angle between two vectors -double CollisionAvoidanceTaskNode::calculate_angle( - const geometry_msgs::msg::Vector3 &twist1, - const geometry_msgs::msg::Vector3 &twist2) { - // Extract linear velocities - double Ax = twist1.x; - double Ay = twist1.y; - double Bx = twist2.x; - double By = twist2.y; - - // Calculate dot product - double dot_product = Ax * Bx + Ay * By; - - // Calculate magnitudes - double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); - double magnitude_B = std::sqrt(Bx * Bx + By * By); - - // Calculate angle in radians - double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); - - // Convert to degrees if needed - double angle_degrees = angle_radians * (180.0 / M_PI); - - return angle_degrees; -} - -bool CollisionAvoidanceTaskNode::has_vessel_passed_freya( - const Eigen::Vector2d &direction_vector_downwards) { - // Calculate the relative vector from freya to the vessel - auto freya_pose = get_odom(); - auto vessel_pose = get_vessel_odom(); - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel - << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, - vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; - - // Project the relative vector onto the direction_vector_downwards - double projection = - direction_vector_freya_to_vessel.dot(direction_vector_downwards); - - // Check the sign of the projection - if (projection < 0) { - // The vessel has passed freya - return true; - } else { - // The vessel has not passed freya yet - return false; - } -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - - odom_start_point_.x, - this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; - direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; - - return predicted_positions; -} - -Eigen::Array CollisionAvoidanceTaskNode::predict_fourth_buoy_pair( - const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green) { - Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - - odom_start_point_.x, - this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; - direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_red.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_green.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_green.y + direction_vector(1) * 5; - - return predicted_positions; -} - -} // namespace collision_avoidance_task \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_task/CMakeLists.txt deleted file mode 100644 index 2ecfb3b2..00000000 --- a/mission/njord_tasks/docking_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(docking_task) - -# 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(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(docking_task_node - src/docking_task_node.cpp - src/docking_task_ros.cpp) - -target_link_libraries(docking_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(docking_task_node - rclcpp - njord_task_base - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions -) - -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/njord_tasks/docking_task/docking_task_1.png b/mission/njord_tasks/docking_task/docking_task_1.png deleted file mode 100644 index c569b626..00000000 Binary files a/mission/njord_tasks/docking_task/docking_task_1.png and /dev/null differ diff --git a/mission/njord_tasks/docking_task/docking_task_2.png b/mission/njord_tasks/docking_task/docking_task_2.png deleted file mode 100644 index b987a94d..00000000 Binary files a/mission/njord_tasks/docking_task/docking_task_2.png and /dev/null differ diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp deleted file mode 100644 index 8463100e..00000000 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#ifndef DOCKING_TASK_ROS_HPP -#define DOCKING_TASK_ROS_HPP - -#include -#include -#include -#include -#include -#include -#include - -namespace docking_task { - -/** - * @class DockingTaskNode - * @brief A class representing a node for handling dock localization in a ROS 2 - * system. - * - * This class inherits from rclcpp::Node and provides functionality for - * localizing the dock in the map. - */ -class DockingTaskNode : public NjordTaskBaseNode { -public: - /** - * @brief Constructor for the DockingTaskNode class. - * - * @param options The options for configuring the node. - */ - explicit DockingTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - /** - * @brief Destructor for the DockLocalizationNode class. - */ - ~DockingTaskNode(){}; - - /** - * @brief Main task for the DockingTaskNode class. - */ - void main_task(); - - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - void grid_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - - std::shared_ptr get_grid(); - - void initialize_grid_sub(); - - std::pair find_dock_structure_edges( - const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up); - - /** - * @brief Iterate over line in grid map. Returns a vector - * representing occupied cells along the line. - * - * @param grid The grid map. - * @param x0 The x-coordinate of the start point. - * @param y0 The y-coordinate of the start point. - * @param x1 The x-coordinate of the end point. - * @param y1 The y-coordinate of the end point. - * @return A vector representing occupied cells along the line. - */ - std::vector search_line(const nav_msgs::msg::OccupancyGrid &grid, - double x0, double y0, double x1, double y1); - -private: - mutable std::mutex grid_mutex_; - bool new_grid_msg_ = false; - std::condition_variable grid_cond_var_; - nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_; - rclcpp::Subscription::SharedPtr grid_sub_; -}; - -} // namespace docking_task - -#endif // DOCKING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/launch/docking_task_launch.py b/mission/njord_tasks/docking_task/launch/docking_task_launch.py deleted file mode 100644 index 20434096..00000000 --- a/mission/njord_tasks/docking_task/launch/docking_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - docking_task_node = Node( - package='docking_task', - executable='docking_task_node', - name='docking_task_node', - parameters=[os.path.join(get_package_share_directory('docking_task'),'params','docking_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - docking_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml deleted file mode 100644 index e3dc286c..00000000 --- a/mission/njord_tasks/docking_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - docking_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml deleted file mode 100644 index 121924a9..00000000 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -docking_task_node: - ros__parameters: - # map_origin_lat: 63.4411585867919 - # map_origin_lon: 10.419400373255625 - map_origin_lat: -33.72242824301795 - map_origin_lon: 150.6740063854522 - map_origin_set: true - gps_start_lat: -33.72255346763595 - gps_start_lon: 150.67394210459224 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 0.0 - gps_end_y: 0.0 - gps_frame_coords_set: false - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters (not derived from base class) - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame - # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it - distance_between_buoys_in_pair: 5.0 # Distance between the two buoys in a pair - grid_topic: "pcl_grid" - dock_structure_absolute_width: 4.39 # Width of the dock structure in meters 4.39 - dock_structure_absolute_width_tolerance: 0.5 # Padding on each side of the dock structure for line search start and end - dock_edge_width: 0.13 # width of the dock edge - line_search_distance: 2.0 # distance to iterate along line when searching for dock edge - dock_search_offset: 2.0 # offset in forward direction from third buoy pair to begin search for dock - - - - - - dock_width: 4.13 # width of available docking space - dock_width_tolerance: 0.5 - dock_length: 2.0 # length of available docking space - dock_length_tolerance: 0.5 - dock_edge_width_tolerance: 0.05 - task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2 - models: - dynmod_stddev: 0.01 - senmod_stddev: 0.01 - \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/src/docking_task_node.cpp b/mission/njord_tasks/docking_task/src/docking_task_node.cpp deleted file mode 100644 index ca84d750..00000000 --- a/mission/njord_tasks/docking_task/src/docking_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - - return 0; -} diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp deleted file mode 100644 index b58d8a38..00000000 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ /dev/null @@ -1,637 +0,0 @@ -#include - -namespace docking_task { - -DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("dock_localization_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("distance_between_buoys_in_pair", 5.0); - declare_parameter("grid_topic", "grid"); - declare_parameter("dock_structure_absolute_width", 0.0); - declare_parameter("dock_structure_absolute_width_tolerance", 0.0); - declare_parameter("dock_edge_width", 0.0); - declare_parameter("line_search_distance", 0.0); - declare_parameter("dock_search_offset", 0.0); - - declare_parameter("dock_width", 0.0); - declare_parameter("dock_width_tolerance", 0.0); - declare_parameter("dock_length", 0.0); - declare_parameter("dock_length_tolerance", 0.0); - declare_parameter("dock_edge_width_tolerance", 0.0); - declare_parameter("task_nr", 0.0); - declare_parameter("models.dynmod_stddev", 0.0); - declare_parameter("models.sen_stddev", 0.0); - - std::thread(&DockingTaskNode::main_task, this).detach(); -} - -void DockingTaskNode::main_task() { - navigation_ready(); - // Starting docking task - odom_start_point_ = get_odom()->pose.pose.position; - // Eigen::Array predicted_first_pair = - // predict_first_buoy_pair(); - - // sensor_msgs::msg::PointCloud2 buoy_vis_msg; - // pcl::PointCloud buoy_vis; - // pcl::PointXYZRGB buoy_red_0; - // buoy_red_0.x = predicted_first_pair(0, 0); - // buoy_red_0.y = predicted_first_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // pcl::PointXYZRGB buoy_green_1; - // buoy_green_1.x = predicted_first_pair(0, 1); - // buoy_green_1.y = predicted_first_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // double distance_to_first_buoy_pair = - // this->get_parameter("distance_to_first_buoy_pair").as_double(); - // if (distance_to_first_buoy_pair > 6.0) { - // geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - // waypoint_to_approach_first_pair_base_link.x = - // distance_to_first_buoy_pair - 4.0; - // waypoint_to_approach_first_pair_base_link.y = 0.0; - // waypoint_to_approach_first_pair_base_link.z = 0.0; - // try { - // auto transform = - // tf_buffer_->lookupTransform("odom", "base_link", - // tf2::TimePointZero); - // geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; - // tf2::doTransform(waypoint_to_approach_first_pair_base_link, - // waypoint_to_approach_first_pair_odom, transform); - // send_waypoint(waypoint_to_approach_first_pair_odom); - // set_desired_heading(odom_start_point_, - // waypoint_to_approach_first_pair_odom); - // reach_waypoint(1.0); - // } catch (tf2::TransformException &ex) { - // RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - // } - // } - // std::vector buoy_landmarks_0_to_1 = - // get_buoy_landmarks(predicted_first_pair); - // if (buoy_landmarks_0_to_1.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // geometry_msgs::msg::Point waypoint_first_pair; - // waypoint_first_pair.x = - // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + - // buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / - // 2; - // waypoint_first_pair.y = - // (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + - // buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / - // 2; - // waypoint_first_pair.z = 0.0; - // send_waypoint(waypoint_first_pair); - // set_desired_heading(odom_start_point_, waypoint_first_pair); - // reach_waypoint(1.0); - - // // Second pair of buoys - // Eigen::Array predicted_first_and_second_pair = - // predict_second_buoy_pair( - // buoy_landmarks_0_to_1[0].pose_odom_frame.position, - // buoy_landmarks_0_to_1[1].pose_odom_frame.position); - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = predicted_first_and_second_pair(0, 0); - // buoy_red_0.y = predicted_first_and_second_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = predicted_first_and_second_pair(0, 1); - // buoy_green_1.y = predicted_first_and_second_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // std::vector buoy_landmarks_2_to_3 = - // get_buoy_landmarks(predicted_first_and_second_pair); - // if (buoy_landmarks_2_to_3.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // geometry_msgs::msg::Point waypoint_second_pair; - // waypoint_second_pair.x = - // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + - // buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / - // 2; - // waypoint_second_pair.y = - // (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + - // buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / - // 2; - // waypoint_second_pair.z = 0.0; - // send_waypoint(waypoint_second_pair); - // set_desired_heading(odom_start_point_, waypoint_second_pair); - // reach_waypoint(1.0); - - // // Third pair of buoys - // Eigen::Array predicted_third_pair = predict_third_buoy_pair( - // buoy_landmarks_0_to_1[0].pose_odom_frame.position, - // buoy_landmarks_0_to_1[1].pose_odom_frame.position, - // buoy_landmarks_2_to_3[2].pose_odom_frame.position, - // buoy_landmarks_2_to_3[3].pose_odom_frame.position); - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = predicted_third_pair(0, 0); - // buoy_red_0.y = predicted_third_pair(1, 0); - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = predicted_third_pair(0, 1); - // buoy_green_1.y = predicted_third_pair(1, 1); - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // std::vector buoy_landmarks_4_to_5 = - // get_buoy_landmarks(predicted_third_pair); - // if (buoy_landmarks_4_to_5.size() != 2) { - // RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - // } - - // buoy_vis = pcl::PointCloud(); - // buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - // buoy_red_0 = pcl::PointXYZRGB(); - // buoy_green_1 = pcl::PointXYZRGB(); - // buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; - // buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; - // buoy_red_0.z = 0.0; - // buoy_red_0.r = 255; - // buoy_red_0.g = 0; - // buoy_red_0.b = 0; - // buoy_vis.push_back(buoy_red_0); - // buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; - // buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; - // buoy_green_1.z = 0.0; - // buoy_green_1.r = 0; - // buoy_green_1.g = 255; - // buoy_green_1.b = 0; - // buoy_vis.push_back(buoy_green_1); - // pcl::toROSMsg(buoy_vis, buoy_vis_msg); - // buoy_vis_msg.header.frame_id = "odom"; - // buoy_vis_msg.header.stamp = this->now(); - // buoy_visualization_pub_->publish(buoy_vis_msg); - - // Eigen::Vector2d direction_vector_up; - // direction_vector_up << (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x - // + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2 - odom_start_point_.x, - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2 - odom_start_point_.y; - // direction_vector_up.normalize(); - - // geometry_msgs::msg::Point waypoint_third_pair; - // waypoint_third_pair.x = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2; - // waypoint_third_pair.y = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2; - // waypoint_third_pair.z = 0.0; - // send_waypoint(waypoint_third_pair); - // set_desired_heading(odom_start_point_, waypoint_third_pair); - - // geometry_msgs::msg::Point waypoint_through_third_pair; - // waypoint_through_third_pair.x = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / - // 2 + direction_vector_up(0) * 2; - // waypoint_through_third_pair.y = - // (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + - // buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / - // 2 + direction_vector_up(1) * 2; - // waypoint_through_third_pair.z = 0.0; - // send_waypoint(waypoint_through_third_pair); - // set_desired_heading(odom_start_point_, waypoint_through_third_pair); - // reach_waypoint(1.0); - - geometry_msgs::msg::Point direction_vector_up_base_link; - direction_vector_up_base_link.x = 1.0; - direction_vector_up_base_link.y = 0.0; - direction_vector_up_base_link.z = 0.0; - Eigen::Vector2d direction_vector_up_test; - try { - auto transform = - tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - geometry_msgs::msg::Point direction_vector_up_odom; - tf2::doTransform(direction_vector_up_base_link, direction_vector_up_odom, - transform); - direction_vector_up_test << direction_vector_up_odom.x, - direction_vector_up_odom.y; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - - std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); - Eigen::Vector2d edge1, edge2; - // std::tie(edge1, edge2) = find_dock_structure_edges(waypoint_third_pair, - // direction_vector_up); - - std::tie(edge1, edge2) = - find_dock_structure_edges(odom_start_point_, direction_vector_up_test); - RCLCPP_INFO(this->get_logger(), "Edge 1: %f, %f", edge1(0), edge1(1)); - RCLCPP_INFO(this->get_logger(), "Edge 2: %f, %f", edge2(0), edge2(1)); - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_vis = pcl::PointCloud(); - pcl::PointXYZRGB edge1_point; - edge1_point.x = edge1(0); - edge1_point.y = edge1(1); - edge1_point.z = 0.0; - edge1_point.r = 255; - edge1_point.g = 0; - edge1_point.b = 0; - pcl::PointXYZRGB edge2_point; - edge2_point.x = edge2(0); - edge2_point.y = edge2(1); - edge2_point.z = 0.0; - edge2_point.r = 0; - edge2_point.g = 255; - edge2_point.b = 0; - buoy_vis.push_back(edge1_point); - buoy_vis.push_back(edge2_point); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); -} - -Eigen::Array DockingTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - double distance_between_buoys_in_pair = - this->get_parameter("distance_between_buoys_in_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -distance_between_buoys_in_pair / 2; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = distance_between_buoys_in_pair / 2; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array DockingTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; - - return predicted_positions; -} - -Eigen::Array DockingTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_first_to_second_pair; - direction_vector_first_to_second_pair - << (buoy_2.x + buoy_3.x) / 2 - (buoy_0.x + buoy_1.x) / 2, - (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; - direction_vector_first_to_second_pair.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_2.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 0) = - buoy_2.y + direction_vector_first_to_second_pair(1) * 5; - predicted_positions(0, 1) = - buoy_3.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 1) = - buoy_3.y + direction_vector_first_to_second_pair(1) * 5; - - return predicted_positions; -} - -void DockingTaskNode::initialize_grid_sub() { - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - std::string grid_topic = this->get_parameter("grid_topic").as_string(); - grid_sub_ = this->create_subscription( - grid_topic, qos_sensor_data, - std::bind(&DockingTaskNode::grid_callback, this, std::placeholders::_1)); -} - -void DockingTaskNode::grid_callback( - const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { - std::unique_lock lock(grid_mutex_); - grid_msg_ = msg; - new_grid_msg_ = true; - lock.unlock(); - grid_cond_var_.notify_one(); -} - -std::shared_ptr DockingTaskNode::get_grid() { - std::unique_lock lock(grid_mutex_); - grid_cond_var_.wait(lock, [this] { return new_grid_msg_; }); - new_grid_msg_ = false; - lock.unlock(); - return grid_msg_; -} - -std::vector -DockingTaskNode::search_line(const nav_msgs::msg::OccupancyGrid &grid, - double dx0, double dy0, double dx1, double dy1) { - int x0 = dx0 / grid.info.resolution + grid.info.width / 2; - int y0 = dy0 / grid.info.resolution + grid.info.height / 2; - int x1 = dx1 / grid.info.resolution + grid.info.width / 2; - int y1 = dy1 / grid.info.resolution + grid.info.height / 2; - - std::vector occupied_cells; - bool steep = std::abs(y1 - y0) > std::abs(x1 - x0); - if (steep) { - occupied_cells.reserve(std::abs(y1 - y0)); - } else { - occupied_cells.reserve(std::abs(x1 - x0)); - } - if (steep) { - std::swap(x0, y0); - std::swap(x1, y1); - } - bool swap = x0 > x1; - if (swap) { - 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) { - occupied_cells.push_back(grid.data[y + x * grid.info.width]); - } - } else { - if (y >= 0 && y < ybounds && x >= 0 && x < xbounds) { - occupied_cells.push_back(grid.data[y * grid.info.width + x]); - } - } - if (error > 0) { - y += ystep; - error -= 2 * (dx - dy); - } else { - error += 2 * dy; - } - } - if (swap) { - std::reverse(occupied_cells.begin(), occupied_cells.end()); - } - return occupied_cells; -} - -std::pair -DockingTaskNode::find_dock_structure_edges( - const geometry_msgs::msg::Point &waypoint_third_pair, - Eigen::Vector2d &direction_vector_up) { - double dock_width_structure_absolute_width = - this->get_parameter("dock_structure_absolute_width").as_double(); - double dock_width_structure_absolute_width_tolerance = - this->get_parameter("dock_structure_absolute_width_tolerance") - .as_double(); - double dock_search_offset = - this->get_parameter("dock_search_offset").as_double(); - Eigen::Vector2d direction_vector_right; - // Rotate direction vector 90 degrees to the right - direction_vector_right << direction_vector_up(1), -direction_vector_up(0); - geometry_msgs::msg::Point waypoint_third_pair_map; - bool transform_success = false; - while (!transform_success) { - try { - auto transform = - tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); - tf2::doTransform(waypoint_third_pair, waypoint_third_pair_map, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - Eigen::Vector2d line_start, line_end; - line_start << waypoint_third_pair_map.x - - (direction_vector_right(0) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y - - (direction_vector_right(1) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(1) * dock_search_offset); - line_end << waypoint_third_pair_map.x + - (direction_vector_right(0) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(0) * dock_search_offset), - waypoint_third_pair_map.y + - (direction_vector_right(1) * - (dock_width_structure_absolute_width / 2 + - dock_width_structure_absolute_width_tolerance)) + - (direction_vector_up(1) * dock_search_offset); - // double line_length = std::sqrt(std::pow(line_end(0) - line_start(0), 2) + - // std::pow(line_end(1) - line_start(1), 2)); - double dock_edge_width = this->get_parameter("dock_edge_width").as_double(); - double dock_width = dock_width_structure_absolute_width - 2 * dock_edge_width; - nav_msgs::msg::MapMetaData grid_info = get_grid()->info; - double line_search_distance = - this->get_parameter("line_search_distance").as_double(); - int search_iterations = - static_cast(line_search_distance / grid_info.resolution); - int result_index = -1; - // int line_vector_size = -1; - while (true) { - std::shared_ptr grid = get_grid(); - std::vector occupied_cells = search_line( - *grid, line_start(0), line_start(1), line_end(0), line_end(1)); - int max_result = std::numeric_limits::min(); - int max_index = -1; - for (int i = 0; i < search_iterations; i++) { - int left_edge_left = i; - int left_edge_right = - left_edge_left + - static_cast(dock_edge_width / grid->info.resolution); - int right_edge_left = - left_edge_right + - static_cast(dock_width / grid->info.resolution); - int right_edge_right = - right_edge_left + - static_cast(dock_edge_width / grid->info.resolution); - int left_edge_occupied_cells = - std::accumulate(occupied_cells.begin() + left_edge_left, - occupied_cells.begin() + left_edge_right, 0); - int right_edge_occupied_cells = - std::accumulate(occupied_cells.begin() + right_edge_left, - occupied_cells.begin() + right_edge_right, 0); - if (left_edge_occupied_cells == 0 || right_edge_occupied_cells == 0) { - continue; - } - if (left_edge_occupied_cells + right_edge_occupied_cells > max_result) { - max_result = left_edge_occupied_cells + right_edge_occupied_cells; - max_index = i; - } - } - if (max_result > 300) { - result_index = max_index; - // line_vector_size = occupied_cells.size(); - } - line_start(0) = - line_start(0) + direction_vector_up(0) * grid_info.resolution; - line_start(1) = - line_start(1) + direction_vector_up(1) * grid_info.resolution; - line_end(0) = line_end(0) + direction_vector_up(0) * grid_info.resolution; - line_end(1) = line_end(1) + direction_vector_up(1) * grid_info.resolution; - } - int right_edge_left_index = - result_index + static_cast(dock_edge_width / grid_info.resolution) + - static_cast(dock_width / grid_info.resolution); - - Eigen::Vector2d dock_edge_left, dock_edge_right; - dock_edge_left << line_start(0) + - (direction_vector_right(0) * result_index * - grid_info.resolution) + - dock_edge_width / 2, - line_start(1) + - (direction_vector_right(1) * result_index * grid_info.resolution) + - dock_edge_width / 2; - dock_edge_right << line_start(0) + - (direction_vector_right(0) * right_edge_left_index * - grid_info.resolution) + - dock_edge_width / 2, - line_start(1) + - (direction_vector_right(1) * right_edge_left_index * - grid_info.resolution) + - dock_edge_width / 2; - - return std::make_pair(dock_edge_left, dock_edge_right); -} - -} // namespace docking_task \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/CMakeLists.txt b/mission/njord_tasks/maneuvering_task/CMakeLists.txt deleted file mode 100644 index 6f3e7a2a..00000000 --- a/mission/njord_tasks/maneuvering_task/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(maneuvering_task) - -# 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(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - -include_directories(include) - -add_executable(maneuvering_task_node - src/maneuvering_task_node.cpp - src/maneuvering_task_ros.cpp) - -target_link_libraries(maneuvering_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(maneuvering_task_node - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - njord_task_base - pcl_conversions -) - -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/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp deleted file mode 100644 index 731d5920..00000000 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef MANEUVERING_TASK_ROS_HPP -#define MANEUVERING_TASK_ROS_HPP - -#include -#include -#include -#include -#include - -namespace maneuvering_task { - -class ManeuveringTaskNode : public NjordTaskBaseNode { -public: - explicit ManeuveringTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~ManeuveringTaskNode(){}; - - /** - * @brief Main task for the ManeuveringTaskNode class. - */ - void main_task(); - - Eigen::Array predict_first_buoy_pair(); - - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - Eigen::Array - predict_next_pair_in_formation(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green, - Eigen::Vector2d direction_vector); - -private: -}; - -} // namespace maneuvering_task - -#endif // MANEUVERING_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py b/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py deleted file mode 100644 index da8a4eac..00000000 --- a/mission/njord_tasks/maneuvering_task/launch/maneuvering_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - maneuvering_task_node = Node( - package='maneuvering_task', - executable='maneuvering_task_node', - name='maneuvering_task_node', - parameters=[os.path.join(get_package_share_directory('maneuvering_task'),'params','maneuvering_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - maneuvering_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf b/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf deleted file mode 100644 index 94df6470..00000000 Binary files a/mission/njord_tasks/maneuvering_task/maneuvering_task.pdf and /dev/null differ diff --git a/mission/njord_tasks/maneuvering_task/package.xml b/mission/njord_tasks/maneuvering_task/package.xml deleted file mode 100644 index 4703ead8..00000000 --- a/mission/njord_tasks/maneuvering_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - maneuvering_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - njord_task_base - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml b/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml deleted file mode 100644 index 64ca40c3..00000000 --- a/mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -maneuvering_task_node: - ros__parameters: - map_origin_lat: -33.72213988382845 - map_origin_lon: 150.67413500672993 - map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 - gps_end_lat: 63.44125901804796 - gps_end_lon: 10.41857835889424 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 141.45540473589617 - gps_end_y: -0.22386536008730218 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame. - # If the distance is greater than five we drive distance-3m towards end before trying to find the first buoy pair - initial_offset: 1.0 # The vertical offset from the second and third buoy pair in meters - nr_of_pair_in_formation: 20 # Number of buoy pairs in formation, excluding the start and end gate at gps points (NB! pdf is not accurate) - \ No newline at end of file diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp deleted file mode 100644 index 0c56e330..00000000 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#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/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp deleted file mode 100644 index a8137e95..00000000 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ /dev/null @@ -1,468 +0,0 @@ -#include - -namespace maneuvering_task { - -ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("maneuvering_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - declare_parameter("initial_offset", 1.0); - declare_parameter("nr_of_pair_in_formation", 11); - - std::thread(&ManeuveringTaskNode::main_task, this).detach(); -} - -void ManeuveringTaskNode::main_task() { - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); - odom_start_point_ = get_odom()->pose.pose.position; - // First buoy pair - Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - // First first buoy pair is far away, should be closer before trying to - // measure it. - double gps_end_x = this->get_parameter("gps_end_x").as_double(); - double gps_end_y = this->get_parameter("gps_end_y").as_double(); - Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - odom_start_point_.x, - gps_end_y - odom_start_point_.y; - direction_vector_to_end.normalize(); - - double distance = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance > 5.0) { - auto odom = get_odom(); - geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance - 3); - waypoint_toward_start.z = 0.0; - send_waypoint(waypoint_toward_start); - reach_waypoint(2.0); - } - - std::vector measured_first_buoy_pair = - get_buoy_landmarks(predicted_first_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (measured_first_buoy_pair[0].pose_odom_frame.position.x + - measured_first_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (measured_first_buoy_pair[0].pose_odom_frame.position.y + - measured_first_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(0.5); - - // Second buoy pair is FAR away, should be closer before trying to measure it. - geometry_msgs::msg::Point waypoint_approach_formation; - waypoint_approach_formation.x = - waypoint_first_pair.x + direction_vector_to_end(0) * 15; - waypoint_approach_formation.y = - waypoint_first_pair.y + direction_vector_to_end(1) * 15; - waypoint_approach_formation.z = 0.0; - send_waypoint(waypoint_approach_formation); - set_desired_heading(waypoint_first_pair, waypoint_approach_formation); - reach_waypoint(1.0); - - // Second buoy pair - Eigen::Array predicted_second_buoy_pair = - predict_second_buoy_pair( - measured_first_buoy_pair[0].pose_odom_frame.position, - measured_first_buoy_pair[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_second_buoy_pair(0, 0); - buoy_red_0.y = predicted_second_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_second_buoy_pair(0, 1); - buoy_green_1.y = predicted_second_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_buoy_2_to_3 = - get_buoy_landmarks(predicted_second_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (measured_buoy_2_to_3[0].pose_odom_frame.position.x + - measured_buoy_2_to_3[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (measured_buoy_2_to_3[0].pose_odom_frame.position.y + - measured_buoy_2_to_3[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(waypoint_approach_formation, waypoint_second_pair); - reach_waypoint(0.5); - - // Setup variable to navigate formation - Eigen::Vector2d direction_vector_forwards; - direction_vector_forwards << (waypoint_second_pair.x - waypoint_first_pair.x), - (waypoint_second_pair.y - waypoint_first_pair.y); - direction_vector_forwards.normalize(); - - Eigen::Vector2d direction_vector_downwards; - direction_vector_downwards - << measured_buoy_2_to_3[1].pose_odom_frame.position.x - - measured_buoy_2_to_3[0].pose_odom_frame.position.x, - measured_buoy_2_to_3[1].pose_odom_frame.position.y - - measured_buoy_2_to_3[0].pose_odom_frame.position.y; - direction_vector_downwards.normalize(); - - Eigen::Vector2d vector_to_next_pair; - vector_to_next_pair - << direction_vector_forwards(0) * 5 + - direction_vector_downwards(0) * - this->get_parameter("initial_offset").as_double(), - direction_vector_forwards(1) * 5 + - direction_vector_downwards(1) * - this->get_parameter("initial_offset").as_double(); - - geometry_msgs::msg::Point red_buoy = - measured_buoy_2_to_3[0].pose_odom_frame.position; - geometry_msgs::msg::Point green_buoy = - measured_buoy_2_to_3[1].pose_odom_frame.position; - geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; - // ASV is at first buoy pair in formation. - // Run loop for the rest of the formation excluding the first pair. - int num_iterations = - this->get_parameter("nr_of_pair_in_formation").as_int() - 1; - - for (int _ = 0; _ < num_iterations; ++_) { - Eigen::Array predicted_next_pair = - predict_next_pair_in_formation(red_buoy, green_buoy, - vector_to_next_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_next_pair(0, 0); - buoy_red_0.y = predicted_next_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_next_pair(0, 1); - buoy_green_1.y = predicted_next_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_next_pair = - get_buoy_landmarks(predicted_next_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_next_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_next_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_next_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_next_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_next_pair; - waypoint_next_pair.x = (measured_next_pair[0].pose_odom_frame.position.x + - measured_next_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_next_pair.y = (measured_next_pair[0].pose_odom_frame.position.y + - measured_next_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_next_pair.z = 0.0; - send_waypoint(waypoint_next_pair); - set_desired_heading(waypoint_prev_pair, waypoint_next_pair); - reach_waypoint(1.0); - red_buoy = measured_next_pair[0].pose_odom_frame.position; - green_buoy = measured_next_pair[1].pose_odom_frame.position; - vector_to_next_pair << waypoint_next_pair.x - waypoint_prev_pair.x, - waypoint_next_pair.y - waypoint_prev_pair.y; - waypoint_prev_pair = waypoint_next_pair; - } - - // ASV is now at the last pair of the buoy formation - // Should move close to end before we try to measure the last pair - auto odom = get_odom(); - double distance_to_end = - std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + - std::pow(odom->pose.pose.position.y - gps_end_y, 2)); - if (distance_to_end > 7.0) { - geometry_msgs::msg::Point waypoint_toward_end; - waypoint_toward_end.x = - odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance_to_end - 4.0); - waypoint_toward_end.y = - odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance_to_end - 4.0); - waypoint_toward_end.z = 0.0; - send_waypoint(waypoint_toward_end); - set_desired_heading(waypoint_prev_pair, waypoint_toward_end); - reach_waypoint(2.0); - } - Eigen::Array predicted_last_buoy_pair; - predicted_last_buoy_pair(0, 0) = - gps_end_x - direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 0) = - gps_end_y - direction_vector_downwards(1) * 2.5; - predicted_last_buoy_pair(0, 1) = - gps_end_x + direction_vector_downwards(0) * 2.5; - predicted_last_buoy_pair(1, 1) = - gps_end_y + direction_vector_downwards(1) * 2.5; - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = predicted_last_buoy_pair(0, 0); - buoy_red_0.y = predicted_last_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = predicted_last_buoy_pair(0, 1); - buoy_green_1.y = predicted_last_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector measured_last_buoy_pair = - get_buoy_landmarks(predicted_last_buoy_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; - buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; - buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_last_pair; - waypoint_last_pair.x = - (measured_last_buoy_pair[0].pose_odom_frame.position.x + - measured_last_buoy_pair[1].pose_odom_frame.position.x) / - 2; - waypoint_last_pair.y = - (measured_last_buoy_pair[0].pose_odom_frame.position.y + - measured_last_buoy_pair[1].pose_odom_frame.position.y) / - 2; - waypoint_last_pair.z = 0.0; - send_waypoint(waypoint_last_pair); - set_desired_heading(waypoint_prev_pair, waypoint_last_pair); - reach_waypoint(1.0); - // Also send waypoint 2m through the last pair - geometry_msgs::msg::Point waypoint_through_last_pair; - waypoint_through_last_pair.x = - waypoint_last_pair.x + direction_vector_to_end(0) * 2; - waypoint_through_last_pair.y = - waypoint_last_pair.y + direction_vector_to_end(1) * 2; - waypoint_through_last_pair.z = 0.0; - send_waypoint(waypoint_through_last_pair); - set_desired_heading(waypoint_last_pair, waypoint_through_last_pair); - // Task is now finished -} - -Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; - - return predicted_positions; -} - -Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( - const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green, - Eigen::Vector2d vector_to_next_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x + vector_to_next_pair(0); - predicted_positions(1, 0) = buoy_red.y + vector_to_next_pair(1); - predicted_positions(0, 1) = buoy_green.x + vector_to_next_pair(0); - predicted_positions(1, 1) = buoy_green.y + vector_to_next_pair(1); - - return predicted_positions; -} - -} // namespace maneuvering_task \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/CMakeLists.txt b/mission/njord_tasks/navigation_task/CMakeLists.txt deleted file mode 100644 index 9d462689..00000000 --- a/mission/njord_tasks/navigation_task/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(navigation_task) - -# 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(njord_task_base REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) - - -include_directories(include) - -add_executable(navigation_task_node - src/navigation_task_node.cpp - src/navigation_task_ros.cpp -) - -target_link_libraries(navigation_task_node - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - pcl_common -) - -ament_target_dependencies(navigation_task_node - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - njord_task_base - pcl_conversions -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(TARGETS - navigation_task_node - DESTINATION lib/${PROJECT_NAME}/ -) - -install(DIRECTORY - launch - params - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp deleted file mode 100644 index e224c3d4..00000000 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ /dev/null @@ -1,143 +0,0 @@ -#ifndef NAVIGATION_TASK_ROS_HPP -#define NAVIGATION_TASK_ROS_HPP - -#include -#include -#include -#include -#include - -namespace navigation_task { - -class NavigationTaskNode : public NjordTaskBaseNode { -public: - explicit NavigationTaskNode( - const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - ~NavigationTaskNode(){}; - - /** - * @brief Main task for the NavigationTaskNode class. - */ - void main_task(); - - /** - * @brief Predict the positions of the first two buoys - * - * @return An Eigen::Array representing the predicted positions - * of the first two buoys - */ - Eigen::Array predict_first_buoy_pair(); - - /** - * @brief Predict the positions of the second buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the second buoy pair - */ - Eigen::Array - predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); - - /** - * @brief Predict the positions of the west buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted position - * of the west buoy - */ - Eigen::Array - predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - /** - * @brief Predict the positions of the third buoy pair by using the two first - * buoy pairs - * - * @return An Eigen::Array representing the predicted positions - * of the third buoy pair - */ - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - /** - * @brief Predict the positions of the north buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted position - * of the north buoy - */ - Eigen::Array predict_north_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the positions of the south buoy by using the two first buoy - * pairs - * - * @return An Eigen::Array representing the predicted positions - * of the north and south buoys - */ - Eigen::Array predict_south_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the positions of the fourth buoy pair by using the third - * buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the fourth buoy pair - */ - Eigen::Array - predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6); - - /** - * @brief Predict the position of the east buoy by using the fourth buoy pair - * and the direction vector from the second to the third buoy pair - * - * @return An Eigen::Array representing the predicted position - * of the east buoy - */ - Eigen::Array predict_east_buoy( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the position of the fifth buoy pair by using the fourth buoy - * pair and the direction vector from the second to the third buoy pair - * - * @return An Eigen::Array representing the predicted positions - * of the fifth buoy pair - */ - Eigen::Array predict_fifth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair); - - /** - * @brief Predict the position of the sixth buoy pair by using the fifth buoy - * pair and fourth buoy pair - * - * @return An Eigen::Array representing the predicted position - * of the sixth buoy pairs - */ - Eigen::Array - predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_12, - const geometry_msgs::msg::Point &buoy_13); -}; - -} // namespace navigation_task - -#endif // NAVIGATION_TASK_ROS_HPP \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py b/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py deleted file mode 100644 index c2dd17c7..00000000 --- a/mission/njord_tasks/navigation_task/launch/navigation_task_launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - navigation_task_node = Node( - package='navigation_task', - executable='navigation_task_node', - name='navigation_task_node', - parameters=[os.path.join(get_package_share_directory('navigation_task'),'params','navigation_task_params.yaml')], - # arguments=['--ros-args', '--log-level', 'DEBUG'], - output='screen', - ) - return LaunchDescription([ - navigation_task_node - ]) \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/navigation_task.pdf b/mission/njord_tasks/navigation_task/navigation_task.pdf deleted file mode 100644 index 9c0357a1..00000000 Binary files a/mission/njord_tasks/navigation_task/navigation_task.pdf and /dev/null differ diff --git a/mission/njord_tasks/navigation_task/package.xml b/mission/njord_tasks/navigation_task/package.xml deleted file mode 100644 index 1c06d25a..00000000 --- a/mission/njord_tasks/navigation_task/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - navigation_task - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - njord_task_base - rclcpp - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - pcl_conversions - - - - ament_cmake - - diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml deleted file mode 100644 index fdc8a50b..00000000 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ /dev/null @@ -1,22 +0,0 @@ -navigation_task_node: - ros__parameters: - map_origin_lat: -33.72242824301795 - map_origin_lon: 150.6740063854522 - map_origin_set: true - gps_start_lat: -33.722553423946906 - gps_start_lon: 150.67394210456843 - gps_end_lat: -33.72253671663162 - gps_end_lon: 150.67413245758723 - gps_start_x: 0.0 - gps_start_y: 0.0 - gps_end_x: 1.1791211357704299 - gps_end_y: 17.485397720327114 - gps_frame_coords_set: true - map_origin_topic: "/map/origin" - odom_topic: "/seapath/odom/ned" - landmark_topic: "landmarks_out" - assignment_confidence: 10 # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct - - # Task specific parameters - distance_to_first_buoy_pair: 2.0 # Distance in x-direction to first buoy pair in meters from current postition (not gps) position in base_link frame - # If the distance is greater than 6.0 we drive distance-4m towards the first buoy pair before trying to find it \ No newline at end of file diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp deleted file mode 100644 index 4fe227ad..00000000 --- a/mission/njord_tasks/navigation_task/src/navigation_task_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#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/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp deleted file mode 100644 index 9398ecac..00000000 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ /dev/null @@ -1,924 +0,0 @@ -#include - -namespace navigation_task { - -NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options) - : NjordTaskBaseNode("navigation_task_node", options) { - - declare_parameter("distance_to_first_buoy_pair", 2.0); - - std::thread(&NavigationTaskNode::main_task, this).detach(); -} - -void NavigationTaskNode::main_task() { - - navigation_ready(); - RCLCPP_INFO(this->get_logger(), "Navigation task started"); - auto odom_msg = get_odom(); - odom_start_point_ = odom_msg->pose.pose.position; - // First pair of buoys - Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); - sensor_msgs::msg::PointCloud2 buoy_vis_msg; - pcl::PointCloud buoy_vis; - - pcl::PointXYZRGB buoy_red_0; - buoy_red_0.x = predicted_first_buoy_pair(0, 0); - buoy_red_0.y = predicted_first_buoy_pair(1, 0); - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - pcl::PointXYZRGB buoy_green_1; - buoy_green_1.x = predicted_first_buoy_pair(0, 1); - buoy_green_1.y = predicted_first_buoy_pair(1, 1); - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - if (distance_to_first_buoy_pair > 6.0) { - geometry_msgs::msg::Point waypoint_to_approach_first_pair_base_link; - waypoint_to_approach_first_pair_base_link.x = - distance_to_first_buoy_pair - 4.0; - waypoint_to_approach_first_pair_base_link.y = 0.0; - waypoint_to_approach_first_pair_base_link.z = 0.0; - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; - tf2::doTransform(waypoint_to_approach_first_pair_base_link, - waypoint_to_approach_first_pair_odom, transform); - send_waypoint(waypoint_to_approach_first_pair_odom); - set_desired_heading(odom_start_point_, - waypoint_to_approach_first_pair_odom); - reach_waypoint(1.0); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - std::vector buoy_landmarks_0_to_1 = - get_buoy_landmarks(predicted_first_buoy_pair); - if (buoy_landmarks_0_to_1.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_0 = pcl::PointXYZRGB(); - buoy_green_1 = pcl::PointXYZRGB(); - buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; - buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; - buoy_red_0.z = 0.0; - buoy_red_0.r = 255; - buoy_red_0.g = 0; - buoy_red_0.b = 0; - buoy_vis.push_back(buoy_red_0); - buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; - buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; - buoy_green_1.z = 0.0; - buoy_green_1.r = 0; - buoy_green_1.g = 255; - buoy_green_1.b = 0; - buoy_vis.push_back(buoy_green_1); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_first_pair; - waypoint_first_pair.x = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.x) / - 2; - waypoint_first_pair.y = - (buoy_landmarks_0_to_1[0].pose_odom_frame.position.y + - buoy_landmarks_0_to_1[1].pose_odom_frame.position.y) / - 2; - waypoint_first_pair.z = 0.0; - send_waypoint(waypoint_first_pair); - set_desired_heading(odom_start_point_, waypoint_first_pair); - reach_waypoint(1.0); - - // Second pair of buoys - Eigen::Array predicted_second_pair = predict_second_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_2; - pcl::PointXYZRGB buoy_green_3; - buoy_red_2.x = predicted_second_pair(0, 0); - buoy_red_2.y = predicted_second_pair(1, 0); - buoy_red_2.z = 0.0; - buoy_red_2.r = 255; - buoy_red_2.g = 0; - buoy_red_2.b = 0; - buoy_vis.push_back(buoy_red_2); - buoy_green_3.x = predicted_second_pair(0, 1); - buoy_green_3.y = predicted_second_pair(1, 1); - buoy_green_3.z = 0.0; - buoy_green_3.r = 0; - buoy_green_3.g = 255; - buoy_green_3.b = 0; - buoy_vis.push_back(buoy_green_3); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_2_to_3 = - get_buoy_landmarks(predicted_second_pair); - if (buoy_landmarks_2_to_3.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_2 = pcl::PointXYZRGB(); - buoy_green_3 = pcl::PointXYZRGB(); - buoy_red_2.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; - buoy_red_2.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; - buoy_red_2.z = 0.0; - buoy_red_2.r = 255; - buoy_red_2.g = 0; - buoy_red_2.b = 0; - buoy_vis.push_back(buoy_red_2); - buoy_green_3.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; - buoy_green_3.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - buoy_green_3.z = 0.0; - buoy_green_3.r = 0; - buoy_green_3.g = 255; - buoy_green_3.b = 0; - buoy_vis.push_back(buoy_green_3); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_second_pair; - waypoint_second_pair.x = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / - 2; - waypoint_second_pair.y = - (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + - buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / - 2; - waypoint_second_pair.z = 0.0; - send_waypoint(waypoint_second_pair); - set_desired_heading(waypoint_first_pair, waypoint_second_pair); - reach_waypoint(1.0); - - // West buoy - Eigen::Array predicted_west_buoy = - predict_west_buoy(buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[0].pose_odom_frame.position, - buoy_landmarks_2_to_3[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_west; - buoy_west.x = predicted_west_buoy(0, 0); - buoy_west.y = predicted_west_buoy(1, 0); - buoy_west.z = 0.0; - buoy_west.r = 255; - buoy_west.g = 255; - buoy_west.b = 0; - buoy_vis.push_back(buoy_west); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_4 = - get_buoy_landmarks(predicted_west_buoy); - if (buoy_landmarks_4.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_west = pcl::PointXYZRGB(); - buoy_west.x = buoy_landmarks_4[0].pose_odom_frame.position.x; - buoy_west.y = buoy_landmarks_4[0].pose_odom_frame.position.y; - buoy_west.z = 0.0; - buoy_west.r = 255; - buoy_west.g = 255; - buoy_west.b = 0; - buoy_vis.push_back(buoy_west); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_upwards; - direction_vector_upwards - << buoy_landmarks_2_to_3[0].pose_odom_frame.position.x - - buoy_landmarks_2_to_3[1].pose_odom_frame.position.x, - buoy_landmarks_2_to_3[0].pose_odom_frame.position.y - - buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; - direction_vector_upwards.normalize(); - - // Set waypoint two meters above west buoy - geometry_msgs::msg::Point waypoint_west_buoy; - waypoint_west_buoy.x = buoy_landmarks_4[0].pose_odom_frame.position.x + - direction_vector_upwards(0) * 3; - waypoint_west_buoy.y = buoy_landmarks_4[0].pose_odom_frame.position.y + - direction_vector_upwards(1) * 3; - waypoint_west_buoy.z = 0.0; - - send_waypoint(waypoint_west_buoy); - set_desired_heading(waypoint_second_pair, waypoint_west_buoy); - reach_waypoint(1.0); - - // Third pair of buoys - Eigen::Array predicted_third_buoy_pair = - predict_third_buoy_pair( - buoy_landmarks_0_to_1[0].pose_odom_frame.position, - buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_2_to_3[0].pose_odom_frame.position, - buoy_landmarks_2_to_3[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_5; - pcl::PointXYZRGB buoy_green_6; - buoy_red_5.x = predicted_third_buoy_pair(0, 0); - buoy_red_5.y = predicted_third_buoy_pair(1, 0); - buoy_red_5.z = 0.0; - buoy_red_5.r = 255; - buoy_red_5.g = 0; - buoy_red_5.b = 0; - buoy_vis.push_back(buoy_red_5); - buoy_green_6.x = predicted_third_buoy_pair(0, 1); - buoy_green_6.y = predicted_third_buoy_pair(1, 1); - buoy_green_6.z = 0.0; - buoy_green_6.r = 0; - buoy_green_6.g = 255; - buoy_green_6.b = 0; - buoy_vis.push_back(buoy_green_6); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_5_to_6 = - get_buoy_landmarks(predicted_third_buoy_pair); - if (buoy_landmarks_5_to_6.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_5 = pcl::PointXYZRGB(); - buoy_green_6 = pcl::PointXYZRGB(); - buoy_red_5.x = buoy_landmarks_5_to_6[0].pose_odom_frame.position.x; - buoy_red_5.y = buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; - buoy_red_5.z = 0.0; - buoy_red_5.r = 255; - buoy_red_5.g = 0; - buoy_red_5.b = 0; - buoy_vis.push_back(buoy_red_5); - buoy_green_6.x = buoy_landmarks_5_to_6[1].pose_odom_frame.position.x; - buoy_green_6.y = buoy_landmarks_5_to_6[1].pose_odom_frame.position.y; - buoy_green_6.z = 0.0; - buoy_green_6.r = 0; - buoy_green_6.g = 255; - buoy_green_6.b = 0; - buoy_vis.push_back(buoy_green_6); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_third_pair; - waypoint_third_pair.x = - (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + - buoy_landmarks_5_to_6[1].pose_odom_frame.position.x) / - 2; - waypoint_third_pair.y = - (buoy_landmarks_5_to_6[0].pose_odom_frame.position.y + - buoy_landmarks_5_to_6[1].pose_odom_frame.position.y) / - 2; - waypoint_third_pair.z = 0.0; - - send_waypoint(waypoint_third_pair); - set_desired_heading(waypoint_west_buoy, waypoint_third_pair); - Eigen::Vector2d direction_vector_second_to_third_pair; - direction_vector_second_to_third_pair - << waypoint_third_pair.x - waypoint_second_pair.x, - waypoint_third_pair.y - waypoint_second_pair.y; - direction_vector_second_to_third_pair.normalize(); - geometry_msgs::msg::Point waypoint_through_third_pair; - waypoint_through_third_pair.x = - waypoint_third_pair.x + direction_vector_second_to_third_pair(0) * 2; - waypoint_through_third_pair.y = - waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; - waypoint_through_third_pair.z = 0.0; - send_waypoint(waypoint_through_third_pair); - set_desired_heading(waypoint_third_pair, waypoint_through_third_pair); - reach_waypoint(1.0); - - // North buoy - Eigen::Array predicted_north_buoy = - predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - std::vector buoy_landmarks_7 = - get_buoy_landmarks(predicted_north_buoy); - if (buoy_landmarks_7.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_north; - buoy_north.x = buoy_landmarks_7[0].pose_odom_frame.position.x; - buoy_north.y = buoy_landmarks_7[0].pose_odom_frame.position.y; - buoy_north.z = 0.0; - buoy_north.r = 255; - buoy_north.g = 255; - buoy_north.b = 0; - buoy_vis.push_back(buoy_north); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_north_buoy; - waypoint_north_buoy.x = buoy_landmarks_7[0].pose_odom_frame.position.x + - direction_vector_second_to_third_pair(0) * 2.5; - waypoint_north_buoy.y = buoy_landmarks_7[0].pose_odom_frame.position.y + - direction_vector_second_to_third_pair(1) * 2.5; - waypoint_north_buoy.z = 0.0; - send_waypoint(waypoint_north_buoy); - set_desired_heading(waypoint_through_third_pair, waypoint_north_buoy); - reach_waypoint(1.0); - - // South buoy - Eigen::Array predicted_south_buoy = - predict_south_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_south; - buoy_south.x = predicted_south_buoy(0, 0); - buoy_south.y = predicted_south_buoy(1, 0); - buoy_south.z = 0.0; - buoy_south.r = 255; - buoy_south.g = 255; - buoy_south.b = 0; - buoy_vis.push_back(buoy_south); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_8 = - get_buoy_landmarks(predicted_south_buoy); - if (buoy_landmarks_8.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_south = pcl::PointXYZRGB(); - buoy_south.x = buoy_landmarks_8[0].pose_odom_frame.position.x; - buoy_south.y = buoy_landmarks_8[0].pose_odom_frame.position.y; - buoy_south.z = 0.0; - buoy_south.r = 255; - buoy_south.g = 255; - buoy_south.b = 0; - buoy_vis.push_back(buoy_south); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_downwards; - direction_vector_downwards - << buoy_landmarks_5_to_6[1].pose_odom_frame.position.x - - buoy_landmarks_5_to_6[10].pose_odom_frame.position.x, - buoy_landmarks_5_to_6[1].pose_odom_frame.position.y - - buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; - direction_vector_downwards.normalize(); - - geometry_msgs::msg::Point waypoint_south_buoy; - waypoint_south_buoy.x = buoy_landmarks_8[0].pose_odom_frame.position.x - - direction_vector_second_to_third_pair(0) * 3 + - direction_vector_downwards(0) * 1; - waypoint_south_buoy.y = buoy_landmarks_8[0].pose_odom_frame.position.y - - direction_vector_second_to_third_pair(1) * 3 + - direction_vector_downwards(1) * 1; - waypoint_south_buoy.z = 0.0; - send_waypoint(waypoint_south_buoy); - set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); - reach_waypoint(0.2); - - // Fourth pair of buoys - Eigen::Array predicted_fourth_buoy_pair = - predict_fourth_buoy_pair( - buoy_landmarks_5_to_6[0].pose_odom_frame.position, - buoy_landmarks_5_to_6[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_9; - pcl::PointXYZRGB buoy_green_10; - buoy_red_9.x = predicted_fourth_buoy_pair(0, 0); - buoy_red_9.y = predicted_fourth_buoy_pair(1, 0); - buoy_red_9.z = 0.0; - buoy_red_9.r = 255; - buoy_red_9.g = 0; - buoy_red_9.b = 0; - buoy_vis.push_back(buoy_red_9); - buoy_green_10.x = predicted_fourth_buoy_pair(0, 1); - buoy_green_10.y = predicted_fourth_buoy_pair(1, 1); - buoy_green_10.z = 0.0; - buoy_green_10.r = 0; - buoy_green_10.g = 255; - buoy_green_10.b = 0; - buoy_vis.push_back(buoy_green_10); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_9_to_10 = - get_buoy_landmarks(predicted_fourth_buoy_pair); - if (buoy_landmarks_9_to_10.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_9 = pcl::PointXYZRGB(); - buoy_green_10 = pcl::PointXYZRGB(); - buoy_red_9.x = buoy_landmarks_9_to_10[0].pose_odom_frame.position.x; - buoy_red_9.y = buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; - buoy_red_9.z = 0.0; - buoy_red_9.r = 255; - buoy_red_9.g = 0; - buoy_red_9.b = 0; - buoy_vis.push_back(buoy_red_9); - buoy_green_10.x = buoy_landmarks_9_to_10[1].pose_odom_frame.position.x; - buoy_green_10.y = buoy_landmarks_9_to_10[1].pose_odom_frame.position.y; - buoy_green_10.z = 0.0; - buoy_green_10.r = 0; - buoy_green_10.g = 255; - buoy_green_10.b = 0; - buoy_vis.push_back(buoy_green_10); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fourth_pair; - waypoint_fourth_pair.x = - (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + - buoy_landmarks_9_to_10[1].pose_odom_frame.position.x) / - 2; - waypoint_fourth_pair.y = - (buoy_landmarks_9_to_10[0].pose_odom_frame.position.y + - buoy_landmarks_9_to_10[1].pose_odom_frame.position.y) / - 2; - waypoint_fourth_pair.z = 0.0; - send_waypoint(waypoint_fourth_pair); - set_desired_heading(waypoint_south_buoy, waypoint_fourth_pair); - reach_waypoint(1.0); - - // East buoy - Eigen::Array predicted_east_buoy = - predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_east; - buoy_east.x = predicted_east_buoy(0, 0); - buoy_east.y = predicted_east_buoy(1, 0); - buoy_east.z = 0.0; - buoy_east.r = 255; - buoy_east.g = 255; - buoy_east.b = 0; - buoy_vis.push_back(buoy_east); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_11 = - get_buoy_landmarks(predicted_east_buoy); - if (buoy_landmarks_11.size() != 1) { - RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_east = pcl::PointXYZRGB(); - buoy_east.x = buoy_landmarks_11[0].pose_odom_frame.position.x; - buoy_east.y = buoy_landmarks_11[0].pose_odom_frame.position.y; - buoy_east.z = 0.0; - buoy_east.r = 255; - buoy_east.g = 255; - buoy_east.b = 0; - buoy_vis.push_back(buoy_east); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - Eigen::Vector2d direction_vector_9_to_10; - direction_vector_9_to_10 - << buoy_landmarks_9_to_10[1].pose_odom_frame.position.x - - buoy_landmarks_9_to_10[0].pose_odom_frame.position.x, - buoy_landmarks_9_to_10[1].pose_odom_frame.position.y - - buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; - direction_vector_9_to_10.normalize(); - geometry_msgs::msg::Point waypoint_east_buoy; - waypoint_east_buoy.x = buoy_landmarks_11[0].pose_odom_frame.position.x + - direction_vector_9_to_10(0) * 3; - waypoint_east_buoy.y = buoy_landmarks_11[0].pose_odom_frame.position.y + - direction_vector_9_to_10(1) * 3; - waypoint_east_buoy.z = 0.0; - send_waypoint(waypoint_east_buoy); - set_desired_heading(waypoint_fourth_pair, waypoint_east_buoy); - reach_waypoint(1.0); - - // Fifth pair of buoys - Eigen::Array predicted_fifth_buoy_pair = - predict_fifth_buoy_pair( - buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - direction_vector_second_to_third_pair); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_12; - pcl::PointXYZRGB buoy_green_13; - buoy_red_12.x = predicted_fifth_buoy_pair(0, 0); - buoy_red_12.y = predicted_fifth_buoy_pair(1, 0); - buoy_red_12.z = 0.0; - buoy_red_12.r = 255; - buoy_red_12.g = 0; - buoy_red_12.b = 0; - buoy_vis.push_back(buoy_red_12); - buoy_green_13.x = predicted_fifth_buoy_pair(0, 1); - buoy_green_13.y = predicted_fifth_buoy_pair(1, 1); - buoy_green_13.z = 0.0; - buoy_green_13.r = 0; - buoy_green_13.g = 255; - buoy_green_13.b = 0; - buoy_vis.push_back(buoy_green_13); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_12_to_13 = - get_buoy_landmarks(predicted_fifth_buoy_pair); - if (buoy_landmarks_12_to_13.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); - } - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_12 = pcl::PointXYZRGB(); - buoy_green_13 = pcl::PointXYZRGB(); - buoy_red_12.x = buoy_landmarks_12_to_13[0].pose_odom_frame.position.x; - buoy_red_12.y = buoy_landmarks_12_to_13[0].pose_odom_frame.position.y; - buoy_red_12.z = 0.0; - buoy_red_12.r = 255; - buoy_red_12.g = 0; - buoy_red_12.b = 0; - buoy_vis.push_back(buoy_red_12); - buoy_green_13.x = buoy_landmarks_12_to_13[1].pose_odom_frame.position.x; - buoy_green_13.y = buoy_landmarks_12_to_13[1].pose_odom_frame.position.y; - buoy_green_13.z = 0.0; - buoy_green_13.r = 0; - buoy_green_13.g = 255; - buoy_green_13.b = 0; - buoy_vis.push_back(buoy_green_13); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_fifth_pair; - waypoint_fifth_pair.x = - (buoy_landmarks_12_to_13[0].pose_odom_frame.position.x + - buoy_landmarks_12_to_13[1].pose_odom_frame.position.x) / - 2; - waypoint_fifth_pair.y = - (buoy_landmarks_12_to_13[0].pose_odom_frame.position.y + - buoy_landmarks_12_to_13[1].pose_odom_frame.position.y) / - 2; - waypoint_fifth_pair.z = 0.0; - send_waypoint(waypoint_fifth_pair); - set_desired_heading(waypoint_east_buoy, waypoint_fifth_pair); - reach_waypoint(1.0); - - // Sixth pair of buoys - Eigen::Array predicted_sixth_buoy_pair = - predict_sixth_buoy_pair( - buoy_landmarks_9_to_10[0].pose_odom_frame.position, - buoy_landmarks_9_to_10[1].pose_odom_frame.position, - buoy_landmarks_12_to_13[0].pose_odom_frame.position, - buoy_landmarks_12_to_13[1].pose_odom_frame.position); - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - pcl::PointXYZRGB buoy_red_14; - pcl::PointXYZRGB buoy_green_15; - buoy_red_14.x = predicted_sixth_buoy_pair(0, 0); - buoy_red_14.y = predicted_sixth_buoy_pair(1, 0); - buoy_red_14.z = 0.0; - buoy_red_14.r = 255; - buoy_red_14.g = 0; - buoy_red_14.b = 0; - buoy_vis.push_back(buoy_red_14); - buoy_green_15.x = predicted_sixth_buoy_pair(0, 1); - buoy_green_15.y = predicted_sixth_buoy_pair(1, 1); - buoy_green_15.z = 0.0; - buoy_green_15.r = 0; - buoy_green_15.g = 255; - buoy_green_15.b = 0; - buoy_vis.push_back(buoy_green_15); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - std::vector buoy_landmarks_14_to_15 = - get_buoy_landmarks(predicted_sixth_buoy_pair); - if (buoy_landmarks_14_to_15.size() != 2) { - RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); - } - - buoy_vis = pcl::PointCloud(); - buoy_vis_msg = sensor_msgs::msg::PointCloud2(); - buoy_red_14 = pcl::PointXYZRGB(); - buoy_green_15 = pcl::PointXYZRGB(); - buoy_red_14.x = buoy_landmarks_14_to_15[0].pose_odom_frame.position.x; - buoy_red_14.y = buoy_landmarks_14_to_15[0].pose_odom_frame.position.y; - buoy_red_14.z = 0.0; - buoy_red_14.r = 255; - buoy_red_14.g = 0; - buoy_red_14.b = 0; - buoy_vis.push_back(buoy_red_14); - buoy_green_15.x = buoy_landmarks_14_to_15[1].pose_odom_frame.position.x; - buoy_green_15.y = buoy_landmarks_14_to_15[1].pose_odom_frame.position.y; - buoy_green_15.z = 0.0; - buoy_green_15.r = 0; - buoy_green_15.g = 255; - buoy_green_15.b = 0; - buoy_vis.push_back(buoy_green_15); - pcl::toROSMsg(buoy_vis, buoy_vis_msg); - buoy_vis_msg.header.frame_id = "odom"; - buoy_vis_msg.header.stamp = this->now(); - buoy_visualization_pub_->publish(buoy_vis_msg); - - geometry_msgs::msg::Point waypoint_sixth_pair; - waypoint_sixth_pair.x = - (buoy_landmarks_14_to_15[0].pose_odom_frame.position.x + - buoy_landmarks_14_to_15[1].pose_odom_frame.position.x) / - 2; - waypoint_sixth_pair.y = - (buoy_landmarks_14_to_15[0].pose_odom_frame.position.y + - buoy_landmarks_14_to_15[1].pose_odom_frame.position.y) / - 2; - waypoint_sixth_pair.z = 0.0; - send_waypoint(waypoint_sixth_pair); - set_desired_heading(waypoint_fifth_pair, waypoint_sixth_pair); - reach_waypoint(1.0); - - // Gps end goal - geometry_msgs::msg::Point gps_end_goal; - gps_end_goal.x = this->get_parameter("gps_end_x").as_double(); - gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); - gps_end_goal.z = 0.0; - send_waypoint(gps_end_goal); - set_desired_heading(waypoint_sixth_pair, gps_end_goal); -} - -Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { - // Predict the positions of the first two buoys - geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; - geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; - buoy_0_base_link_frame.header.frame_id = "base_link"; - buoy_1_base_link_frame.header.frame_id = "base_link"; - - double distance_to_first_buoy_pair = - this->get_parameter("distance_to_first_buoy_pair").as_double(); - - buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_0_base_link_frame.pose.position.y = -2.5; - buoy_0_base_link_frame.pose.position.z = 0.0; - buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair; - buoy_1_base_link_frame.pose.position.y = 2.5; - buoy_1_base_link_frame.pose.position.z = 0.0; - - geometry_msgs::msg::PoseStamped buoy_0_odom_frame; - geometry_msgs::msg::PoseStamped buoy_1_odom_frame; - - bool transform_success = false; - while (!transform_success) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); - tf2::doTransform(buoy_0_base_link_frame, buoy_0_odom_frame, transform); - tf2::doTransform(buoy_1_base_link_frame, buoy_1_odom_frame, transform); - transform_success = true; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x; - predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y; - predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x; - predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_second_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1) { - Eigen::Vector2d direction_vector; - direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, - previous_waypoint_odom_frame_.y - odom_start_point_.y; - direction_vector.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; - - return predicted_positions; -} - -Eigen::Array -NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - - (buoy_1.x + buoy_0.x) / 2, - (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; - direction_vector_right.normalize(); - - Eigen::Vector2d direction_vector_up; - direction_vector_up << (buoy_0.x + buoy_2.x) / 2 - (buoy_1.x + buoy_3.x) / 2, - (buoy_0.y + buoy_2.y) / 2 - (buoy_1.y + buoy_3.y) / 2; - direction_vector_up.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_3.x + - direction_vector_right(0) * 12.5 * 1 / 2 + - direction_vector_up(0) * 5 * 2 / 5; - predicted_positions(1, 0) = buoy_3.y + - direction_vector_right(1) * 12.5 * 1 / 2 + - direction_vector_up(1) * 5 * 2 / 5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_third_buoy_pair( - const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3) { - Eigen::Vector2d direction_vector_right; - direction_vector_right << (buoy_3.x + buoy_2.x) / 2 - - (buoy_1.x + buoy_0.x) / 2, - (buoy_3.y + buoy_2.y) / 2 - (buoy_1.y + buoy_0.y) / 2; - direction_vector_right.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x + direction_vector_right(0) * 12.5; - predicted_positions(1, 0) = buoy_2.y + direction_vector_right(1) * 12.5; - predicted_positions(0, 1) = buoy_3.x + direction_vector_right(0) * 12.5; - predicted_positions(1, 1) = buoy_3.y + direction_vector_right(1) * 12.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_north_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + - direction_vector_second_to_third_pair(0) * 2; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + - direction_vector_second_to_third_pair(1) * 2; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_south_buoy( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 + - direction_vector_second_to_third_pair(0) * 8; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 + - direction_vector_second_to_third_pair(1) * 8; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair( - const geometry_msgs::msg::Point &buoy_5, - const geometry_msgs::msg::Point &buoy_6) { - Eigen::Vector2d direction_5_to_6; - direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; - direction_5_to_6.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5; - predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5; - predicted_positions(0, 1) = - buoy_6.x + direction_5_to_6(0) * 12.5 + direction_5_to_6(0) * 5; - predicted_positions(1, 1) = - buoy_6.y + direction_5_to_6(1) * 12.5 + direction_5_to_6(1) * 5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_east_buoy( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Vector2d direction_9_to_10; - direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; - direction_9_to_10.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_9.x + direction_9_to_10(0) * 2.5 - - direction_vector_second_to_third_pair(0) * 7.5; - predicted_positions(1, 0) = buoy_9.y + direction_9_to_10(1) * 2.5 - - direction_vector_second_to_third_pair(1) * 7.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const Eigen::Vector2d &direction_vector_second_to_third_pair) { - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = - buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 0) = - buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; - predicted_positions(0, 1) = - buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 1) = - buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; - - return predicted_positions; -} - -Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( - const geometry_msgs::msg::Point &buoy_9, - const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_12, - const geometry_msgs::msg::Point &buoy_13) { - Eigen::Vector2d direction_fourth_to_fifth_pair; - direction_fourth_to_fifth_pair - << (buoy_13.x + buoy_12.x) / 2 - (buoy_10.x + buoy_9.x) / 2, - (buoy_13.y + buoy_12.y) / 2 - (buoy_10.y + buoy_9.y) / 2; - direction_fourth_to_fifth_pair.normalize(); - - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 0) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; - predicted_positions(0, 1) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 1) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; - - return predicted_positions; -} - -} // namespace navigation_task \ No newline at end of file diff --git a/mission/njord_tasks/njord_task_base/CMakeLists.txt b/mission/njord_tasks/njord_task_base/CMakeLists.txt deleted file mode 100644 index 2666fae7..00000000 --- a/mission/njord_tasks/njord_task_base/CMakeLists.txt +++ /dev/null @@ -1,85 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(njord_task_base) - -# 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(vortex_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -include_directories(include) - -# Create library target -add_library(${PROJECT_NAME}_lib - src/njord_task_base_ros.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_lib PUBLIC - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs -) - -target_link_libraries(${PROJECT_NAME}_lib PUBLIC - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs -) - -# Specify the include directories for the library -target_include_directories(${PROJECT_NAME}_lib PUBLIC - "$" - "$" -) - -install(TARGETS ${PROJECT_NAME}_lib - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# Export the library -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs -) - -install( - DIRECTORY include/ - DESTINATION include -) - -ament_package() diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp deleted file mode 100644 index d0c55429..00000000 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ /dev/null @@ -1,162 +0,0 @@ -#ifndef NJORD_TASK_BASE_ROS_HPP -#define NJORD_TASK_BASE_ROS_HPP - -#include "geometry_msgs/msg/pose_array.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "vortex_msgs/msg/landmark_array.hpp" -#include "vortex_msgs/srv/desired_velocity.hpp" -#include "vortex_msgs/srv/waypoint.hpp" -#include -#include -#include -#include -#include - -/** - * @brief A struct representing a landmark with id and pose in the odom frame - */ -struct LandmarkPoseID { - geometry_msgs::msg::Pose pose_odom_frame; - int32_t id; -}; - -class NjordTaskBaseNode : public rclcpp::Node { -public: - NjordTaskBaseNode(const std::string &node_name, - const rclcpp::NodeOptions &options); - -protected: - std::pair lla2flat(double lat, double lon) const; - - void map_origin_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg); - - /** - * @brief Runs until the navigation systeam required for task completion is - * ready. This includes setting the map origin, getting the map to odom tf, - * and and getting the odom coordinates of the start and end GPS points. - */ - void navigation_ready(); - - /** - * @brief Spins until the map to odom tf is available. - * Stores the tf in the member variable map_odom_tf_ - * Then initializes the odom and landmark subscribers - */ - void get_map_odom_tf(); - - void initialize_subscribers(); - - /** - * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member - * variables Requires that the map_origin_ member variable is correctly set - */ - void set_gps_odom_points(); - - /** - * @brief Get the odometry message - * If no odometry message is available, the function will wait for a new - * message - * - * @return A shared pointer to the odometry message - */ - std::shared_ptr get_odom(); - - /** - * @brief Get the landmarks in the odom frame - * If no landmarks are available, the function will wait for a new message - * - * @return A shared pointer to the landmarks in the odom frame - */ - std::shared_ptr get_landmarks_odom_frame(); - - /** - * @brief Assign buoys to landmarks based on the predicted and measured - * positions using the auction algorithm. - * - * @param predicted_positions The predicted positions of the buoys - * @param measured_positions The measured positions of landmarks - * - * @return A vector of size equal to number of predicted positions(buoys), - * where each element is the index of the measured position(landmark) assigned - * to the corresponding predicted position(buoy). If an element is -1, it - * means that the corresponding predicted position(buoy) is not assigned to - * any measured position(landmark). - */ - Eigen::VectorXi auction_algorithm( - const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions); - - /** - * @brief Use predicted positions of buoys to get the landmarks with id and - * pose corresponding to the buoys. - * @param predicted_positions The predicted positions of the buoys - * @return A vector of landmarks with id and pose where the index in the - * vector corresponds to the index of the predicted position in the - * predicted_positions array. - */ - std::vector get_buoy_landmarks( - const Eigen::Array &predicted_positions); - - /** - * @brief Send a waypoint to the waypoint service - * @param waypoint The waypoint to send in odom frame - * Also sets the member variable previous_waypoint_odom_frame_ to the waypoint - */ - void send_waypoint(const geometry_msgs::msg::Point &waypoint); - - /** - * @brief Utility function that returns when within distance_threshold of the - * waypoint. The reference waypoint is the member variable - * previous_waypoint_odom_frame_ set by the send_waypoint function. - * - * @param distance_threshold The distance threshold for reaching the waypoint - */ - void reach_waypoint(const double distance_threshold); - - void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, - const geometry_msgs::msg::Point &next_waypoint); - - rclcpp::Publisher::SharedPtr - gps_map_coord_visualization_pub_; - rclcpp::Publisher::SharedPtr - waypoint_visualization_pub_; - rclcpp::Publisher::SharedPtr - buoy_visualization_pub_; - rclcpp::Subscription::SharedPtr map_origin_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr - landmarks_sub_; - rclcpp::Client::SharedPtr waypoint_client_; - rclcpp::Client::SharedPtr heading_client_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - geometry_msgs::msg::TransformStamped map_odom_tf_; - - mutable std::mutex navigation_mutex_; - mutable std::mutex odom_mutex_; - mutable std::mutex landmark_mutex_; - - std::condition_variable odom_cond_var_; - std::condition_variable landmark_cond_var_; - - std::shared_ptr odom_msg_; - std::shared_ptr landmarks_msg_; - bool new_odom_msg_ = false; - bool new_landmark_msg_ = false; - bool navigation_ready_ = false; - - geometry_msgs::msg::Point previous_waypoint_odom_frame_; - geometry_msgs::msg::Point odom_start_point_; -}; - -#endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/package.xml b/mission/njord_tasks/njord_task_base/package.xml deleted file mode 100644 index 12b43f79..00000000 --- a/mission/njord_tasks/njord_task_base/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - njord_task_base - 0.0.0 - TODO: Package description - jorgen - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - nav_msgs - geometry_msgs - sensor_msgs - vortex_msgs - tf2 - tf2_ros - tf2_geometry_msgs - - - - ament_cmake - - diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp deleted file mode 100644 index 8a547d6b..00000000 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ /dev/null @@ -1,532 +0,0 @@ -#include - -NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, - const rclcpp::NodeOptions &options) - : Node(node_name, options) { - declare_parameter("map_origin_lat", 0.0); - declare_parameter("map_origin_lon", 0.0); - declare_parameter("map_origin_set", false); - declare_parameter("gps_start_lat", 0.0); - declare_parameter("gps_start_lon", 0.0); - declare_parameter("gps_end_lat", 0.0); - declare_parameter("gps_end_lon", 0.0); - declare_parameter("gps_start_x", 0.0); - declare_parameter("gps_start_y", 0.0); - declare_parameter("gps_end_x", 0.0); - declare_parameter("gps_end_y", 0.0); - declare_parameter("gps_frame_coords_set", false); - declare_parameter("assignment_confidence", 10); - - declare_parameter("map_origin_topic", "/map/origin"); - declare_parameter("odom_topic", "/seapath/odom/ned"); - declare_parameter("landmark_topic", "landmarks_out"); - - // Sensor data QoS profile - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - - // Transient local QoS profile - 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); - - gps_map_coord_visualization_pub_ = - this->create_publisher( - "/gps_map_coord_visualization", qos_sensor_data); - - buoy_visualization_pub_ = - this->create_publisher( - "/buoy_visualization", qos_sensor_data); - - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - if (this->get_parameter("map_origin_set").as_bool()) { - get_map_odom_tf(); - set_gps_odom_points(); - initialize_subscribers(); - std::unique_lock setup_lock(navigation_mutex_); - navigation_ready_ = true; - setup_lock.unlock(); - } else { - map_origin_sub_ = this->create_subscription( - get_parameter("map_origin_topic").as_string(), qos_transient_local, - std::bind(&NjordTaskBaseNode::map_origin_callback, this, - std::placeholders::_1)); - } - - waypoint_visualization_pub_ = - this->create_publisher( - "/waypoint_visualization", qos_sensor_data); - - waypoint_client_ = - this->create_client("waypoint_list"); - - heading_client_ = - this->create_client("yaw_reference"); -} - -void NjordTaskBaseNode::get_map_odom_tf() { - // Get the transform between the map and odom frames to avoid the overhead - // from continuously looking up the static transform between map and odom - bool tf_set = false; - while (!tf_set) { - try { - auto transform = tf_buffer_->lookupTransform( - "odom", "map", tf2::TimePointZero, tf2::durationFromSec(1.0)); - map_odom_tf_ = transform; - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - std::this_thread::sleep_for(std::chrono::seconds(1)); - continue; - } - tf_set = true; - } -} - -void NjordTaskBaseNode::initialize_subscribers() { - // Sensor data QoS profile - rmw_qos_profile_t qos_profile_sensor_data = rmw_qos_profile_sensor_data; - auto qos_sensor_data = - rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sensor_data.history, 1), - qos_profile_sensor_data); - - odom_sub_ = this->create_subscription( - get_parameter("odom_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::odom_callback, this, - std::placeholders::_1)); - - landmarks_sub_ = this->create_subscription( - get_parameter("landmark_topic").as_string(), qos_sensor_data, - std::bind(&NjordTaskBaseNode::landmark_callback, this, - std::placeholders::_1)); -} - -void NjordTaskBaseNode::set_gps_odom_points() { - if (this->get_parameter("gps_frame_coords_set").as_bool()) { - RCLCPP_INFO(this->get_logger(), "Using predefined GPS frame coordinates"); - geometry_msgs::msg::PoseArray gps_points_odom_frame; - gps_points_odom_frame.header.frame_id = "odom"; - geometry_msgs::msg::Pose gps_start_odom_frame; - gps_start_odom_frame.position.x = - this->get_parameter("gps_start_x").as_double(); - gps_start_odom_frame.position.y = - this->get_parameter("gps_start_y").as_double(); - geometry_msgs::msg::Pose gps_end_odom_frame; - gps_end_odom_frame.position.x = - this->get_parameter("gps_end_x").as_double(); - gps_end_odom_frame.position.y = - this->get_parameter("gps_end_y").as_double(); - gps_points_odom_frame.poses.push_back(gps_start_odom_frame); - gps_points_odom_frame.poses.push_back(gps_end_odom_frame); - gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); - RCLCPP_INFO( - this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, - gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); - return; - } - auto [gps_start_x, gps_start_y] = - lla2flat(this->get_parameter("gps_start_lat").as_double(), - this->get_parameter("gps_start_lon").as_double()); - auto [gps_end_x, gps_end_y] = - lla2flat(this->get_parameter("gps_end_lat").as_double(), - this->get_parameter("gps_end_lon").as_double()); - - geometry_msgs::msg::PoseStamped gps_start_map_frame; - gps_start_map_frame.header.frame_id = "map"; - gps_start_map_frame.pose.position.x = gps_start_x; - gps_start_map_frame.pose.position.y = gps_start_y; - geometry_msgs::msg::PoseStamped gps_end_map_frame; - gps_end_map_frame.header.frame_id = "map"; - gps_end_map_frame.pose.position.x = gps_end_x; - gps_end_map_frame.pose.position.y = gps_end_y; - geometry_msgs::msg::PoseStamped gps_start_odom_frame; - geometry_msgs::msg::PoseStamped gps_end_odom_frame; - try { - tf2::doTransform(gps_start_map_frame, gps_start_odom_frame, map_odom_tf_); - tf2::doTransform(gps_end_map_frame, gps_end_odom_frame, map_odom_tf_); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what()); - return; - } - - this->set_parameter( - rclcpp::Parameter("gps_start_x", gps_start_odom_frame.pose.position.x)); - this->set_parameter( - rclcpp::Parameter("gps_start_y", gps_start_odom_frame.pose.position.y)); - this->set_parameter( - rclcpp::Parameter("gps_end_x", gps_end_odom_frame.pose.position.x)); - this->set_parameter( - rclcpp::Parameter("gps_end_y", gps_end_odom_frame.pose.position.y)); - this->set_parameter(rclcpp::Parameter("gps_frame_coords_set", true)); - RCLCPP_INFO( - this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.pose.position.x, - gps_start_odom_frame.pose.position.y, gps_end_odom_frame.pose.position.x, - gps_end_odom_frame.pose.position.y); - - geometry_msgs::msg::PoseArray gps_points_odom_frame; - gps_points_odom_frame.header.frame_id = "odom"; - - gps_points_odom_frame.poses.push_back(gps_start_odom_frame.pose); - gps_points_odom_frame.poses.push_back(gps_end_odom_frame.pose); - - gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); -} - -std::pair NjordTaskBaseNode::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; - - // Convert angles from degrees to radians - const double lat_rad = lat * M_PI / 180.0; - const double lon_rad = lon * M_PI / 180.0; - const double origin_lat_rad = - this->get_parameter("map_origin_lat").as_double() * M_PI / 180.0; - const double origin_lon_rad = - this->get_parameter("map_origin_lon").as_double() * M_PI / 180.0; - - // 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 NjordTaskBaseNode::map_origin_callback( - const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - this->set_parameter(rclcpp::Parameter("map_origin_lat", msg->latitude)); - this->set_parameter(rclcpp::Parameter("map_origin_lon", msg->longitude)); - this->set_parameter(rclcpp::Parameter("map_origin_set", true)); - RCLCPP_INFO(this->get_logger(), "Map origin set to: %f, %f", msg->latitude, - msg->longitude); - - // Set the map to odom transform - get_map_odom_tf(); - // Set GPS frame coordinates - set_gps_odom_points(); - initialize_subscribers(); - map_origin_sub_.reset(); - std::unique_lock setup_lock(navigation_mutex_); - navigation_ready_ = true; - setup_lock.unlock(); -} - -void NjordTaskBaseNode::navigation_ready() { - bool ready = false; - while (!ready) { - std::unique_lock setup_lock(navigation_mutex_); - ready = navigation_ready_; - setup_lock.unlock(); - RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Waiting for navigation system to be ready"); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } -} - -void NjordTaskBaseNode::odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - std::unique_lock lock(odom_mutex_); - odom_msg_ = msg; - new_odom_msg_ = true; - lock.unlock(); - odom_cond_var_.notify_one(); -} - -std::shared_ptr NjordTaskBaseNode::get_odom() { - std::unique_lock lock(odom_mutex_); - odom_cond_var_.wait(lock, [this] { return new_odom_msg_; }); - new_odom_msg_ = false; - lock.unlock(); - return odom_msg_; -} - -void NjordTaskBaseNode::landmark_callback( - const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { - std::unique_lock lock(landmark_mutex_); - // transform the landmarks to the odom frame - for (auto &landmark : msg->landmarks) { - tf2::doTransform(landmark.odom.pose.pose, landmark.odom.pose.pose, - map_odom_tf_); - } - landmarks_msg_ = msg; - new_landmark_msg_ = true; - lock.unlock(); - landmark_cond_var_.notify_one(); -} - -std::shared_ptr -NjordTaskBaseNode::get_landmarks_odom_frame() { - std::unique_lock lock(landmark_mutex_); - landmark_cond_var_.wait(lock, [this] { return new_landmark_msg_; }); - new_landmark_msg_ = false; - lock.unlock(); - return landmarks_msg_; -} - -Eigen::VectorXi NjordTaskBaseNode::auction_algorithm( - const Eigen::Array &predicted_positions, - const Eigen::Array &measured_positions) { - int num_predicted = predicted_positions.cols(); - int num_measured = measured_positions.cols(); - Eigen::MatrixXd reward_matrix(num_measured, num_predicted); - - if (num_predicted > num_measured) { - RCLCPP_ERROR(this->get_logger(), - "Number of predicted positions is greater than number of " - "measured positions in auction algorithm"); - return Eigen::VectorXi::Constant(num_predicted, -1); - } - - double epsilon = 1e-6; // Small positive number to prevent division by zero - for (Eigen::Index i = 0; i < num_measured; ++i) { - for (Eigen::Index j = 0; j < num_predicted; ++j) { - double dx = measured_positions(0, i) - predicted_positions(0, j); - double dy = measured_positions(1, i) - predicted_positions(1, j); - double distance = std::sqrt(dx * dx + dy * dy); - reward_matrix(i, j) = 1 / (distance + epsilon); - } - } - - Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_predicted, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_measured); - - std::vector unassigned; - for (int i = 0; i < num_predicted; ++i) { - unassigned.push_back(i); - } - - epsilon = 1.0 / (num_measured + num_predicted + 1); - - while (!unassigned.empty()) { - int customer = unassigned.back(); - unassigned.pop_back(); - - double max_value = std::numeric_limits::lowest(); - double second_max_value = std::numeric_limits::lowest(); - int max_item = -1; - - for (int item = 0; item < num_measured; ++item) { - double value = reward_matrix.coeff(item, customer) - prices[item]; - if (value > max_value) { - second_max_value = max_value; - max_value = value; - max_item = item; - } else if (value > second_max_value) { - second_max_value = value; - } - } - - int current_owner = -1; - for (int i = 0; i < num_predicted; ++i) { - if (assignment[i] == max_item) { - current_owner = i; - break; - } - } - if (current_owner != -1) { - unassigned.push_back(current_owner); - } - - assignment[customer] = max_item; - prices[max_item] += max_value - second_max_value + epsilon; - } - return assignment; -} - -std::vector NjordTaskBaseNode::get_buoy_landmarks( - const Eigen::Array &predicted_positions) { - std::vector landmark_ids; - std::vector expected_assignment; - Eigen::Array returned_buoy_positions( - 2, predicted_positions.cols()); - int confidence_threshold = - this->get_parameter("assignment_confidence").as_int(); - int result = 0; - while (result < confidence_threshold) { - landmark_ids.clear(); - auto landmark_msg = get_landmarks_odom_frame(); - - // Extract measured positions and corresponding landmark ids - Eigen::Array measured_positions( - 2, landmark_msg->landmarks.size()); - for (size_t i = 0; i < landmark_msg->landmarks.size(); i++) { - landmark_ids.push_back(landmark_msg->landmarks[i].id); - measured_positions(0, i) = - landmark_msg->landmarks[i].odom.pose.pose.position.x; - measured_positions(1, i) = - landmark_msg->landmarks[i].odom.pose.pose.position.y; - } - - // Check if there are enough landmarks detected to perform auction algorithm - if (predicted_positions.cols() > measured_positions.cols()) { - RCLCPP_ERROR( - this->get_logger(), - "Not enough landmarks detected to perform auction algorithm"); - result = 0; - continue; - } - // Perform auction algorithm - Eigen::VectorXi assignment = - auction_algorithm(predicted_positions, measured_positions); - - // Extract measured positions of assigned buoys - for (Eigen::Index i = 0; i < assignment.size(); i++) { - returned_buoy_positions(0, i) = measured_positions(0, assignment(i)); - returned_buoy_positions(1, i) = measured_positions(1, assignment(i)); - } - - // Check if any buoys are unassigned - // Should never happen as long as the number of landmarks detected is - // greater than or equal to the number of buoys - bool unassigned_buoy = false; - for (Eigen::Index i = 0; i < assignment.size(); i++) { - if (assignment(i) == -1) { - unassigned_buoy = true; - break; - } - } - - // If any buoys are unassigned, restart assignment process - if (unassigned_buoy) { - result = 0; - continue; - } - - // If this is the first iteration, save the assignment and continue - if (result == 0) { - expected_assignment.clear(); - for (Eigen::Index i = 0; i < assignment.size(); i++) { - expected_assignment.push_back(landmark_ids.at(assignment(i))); - } - result++; - continue; - } - - // Check if the assignment is consistent with the previous assignment - bool consistent_assignment = true; - for (Eigen::Index i = 0; i < assignment.size(); i++) { - if (landmark_ids.at(assignment(i)) != expected_assignment.at(i)) { - consistent_assignment = false; - break; - } - } - - // If the assignment is consistent, increment the result counter - // Otherwise, reset the result counter - if (consistent_assignment) { - result++; - continue; - } else { - result = 0; - continue; - } - } - // Loop has completed, return the id and pose of the landmarks assigned to - // buoys - std::vector buoys; - for (size_t i = 0; i < expected_assignment.size(); i++) { - LandmarkPoseID landmark; - landmark.id = expected_assignment.at(i); - landmark.pose_odom_frame.position.x = returned_buoy_positions(0, i); - landmark.pose_odom_frame.position.y = returned_buoy_positions(1, i); - buoys.push_back(landmark); - } - return buoys; -} - -void NjordTaskBaseNode::send_waypoint( - const geometry_msgs::msg::Point &waypoint) { - auto request = std::make_shared(); - request->waypoint.push_back(waypoint); - auto result_future = waypoint_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Waypoint(odom frame) sent: %f, %f", - waypoint.x, waypoint.y); - // Check if the service was successful - double dx = waypoint.x - previous_waypoint_odom_frame_.x; - double dy = waypoint.y - previous_waypoint_odom_frame_.y; - double desired_heading = std::atan2(dy, dx); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, desired_heading); - - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint.x; - waypoint_vis.pose.position.y = waypoint.y; - waypoint_vis.pose.orientation = tf2::toMsg(q); - waypoint_visualization_pub_->publish(waypoint_vis); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); - } - - previous_waypoint_odom_frame_ = waypoint; -} - -void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { - RCLCPP_INFO(this->get_logger(), "Reach waypoint running"); - - auto get_current_position = [&]() { - auto odom_msg = get_odom(); - return std::make_pair(odom_msg->pose.pose.position.x, - odom_msg->pose.pose.position.y); - }; - - auto [x, y] = get_current_position(); - - while (std::hypot(x - previous_waypoint_odom_frame_.x, - y - previous_waypoint_odom_frame_.y) > distance_threshold) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - std::tie(x, y) = get_current_position(); - } - - RCLCPP_INFO(this->get_logger(), "Reached waypoint"); -} - -void NjordTaskBaseNode::set_desired_heading( - const geometry_msgs::msg::Point &prev_waypoint, - const geometry_msgs::msg::Point &next_waypoint) { - auto request = std::make_shared(); - double dx = next_waypoint.x - prev_waypoint.x; - double dy = next_waypoint.y - prev_waypoint.y; - double desired_heading = std::atan2(dy, dx); - request->u_desired = desired_heading; - auto result_future = heading_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "Desired heading sent: %f", desired_heading); - auto status = result_future.wait_for(std::chrono::seconds(5)); - while (status == std::future_status::timeout) { - RCLCPP_INFO(this->get_logger(), "Desired heading service timed out"); - status = result_future.wait_for(std::chrono::seconds(5)); - } - if (!result_future.get()->success) { - RCLCPP_INFO(this->get_logger(), "Desired heading service failed"); - } -} \ No newline at end of file diff --git a/motion/lqr_controller/CMakeLists.txt b/motion/lqr_controller/CMakeLists.txt deleted file mode 100644 index 874cf293..00000000 --- a/motion/lqr_controller/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(lqr_controller) - -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_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - lqr_controller/lqr_controller_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml deleted file mode 100644 index ac497221..00000000 --- a/motion/lqr_controller/config/lqr_config.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - lqr_controller: - Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0] # State costs for [x, y, heading, surge, sway, yaw] - R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py deleted file mode 100644 index 03ceca10..00000000 --- a/motion/lqr_controller/launch/lqr_controller.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - lqr_controller_node = Node( - package='lqr_controller', - executable='lqr_controller_node.py', - name='lqr_controller_node', - parameters=[ - os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'), - os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') - ], - output='screen', - ) - return LaunchDescription([ - lqr_controller_node - ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/lqr_controller/conversions.py b/motion/lqr_controller/lqr_controller/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/motion/lqr_controller/lqr_controller/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py deleted file mode 100644 index d9507b67..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ /dev/null @@ -1,42 +0,0 @@ -import numpy as np -from scipy.linalg import solve_continuous_are - -class LQRController: - def __init__(self): - self.M = np.eye(3) - self.D = np.eye(3) - self.Q = np.eye(6) - self.R = np.eye(3) - - self.A = np.zeros((6,6)) - self.B = np.zeros((6,3)) - self.C = np.zeros((3,6)) - self.C[:3, :3] = np.eye(3) - - def update_parameters(self, M: float, D: list[float], Q: list[float], R: list[float]): - self.M = M - self.M_inv = np.linalg.inv(self.M) - self.D = D - - self.Q = np.diag(Q) - self.R = np.diag(R) - - def calculate_control_input(self, x, x_ref): - tau = -self.K_LQR @ x + self.K_r @ x_ref - return tau - - def calculate_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - rotation_matrix = np.array( - [[np.cos(heading), -np.sin(heading), 0], - [np.sin(heading), np.cos(heading), 0], - [0, 0, 1]] - ) - - self.A[:3,3:] = rotation_matrix - self.A[3:, 3:] = - self.M_inv @ self.D - - self.B[3:,:] = self.M_inv - - self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) - self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) - self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py deleted file mode 100755 index 6ffd5420..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from lqr_controller import LQRController -from conversions import odometrymsg_to_state -from time import sleep -from std_msgs.msg import Float32 -from rcl_interfaces.msg import SetParametersResult - -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class LQRControllerNode(Node): - def __init__(self): - super().__init__("lqr_controller_node") - - self.declare_parameters( - namespace='', - parameters=[ - ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]), - ('lqr_controller.R', [1.0, 1.0, 1.0]), - ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]), - ('physical.damping_matrix_diag', [77.55, 162.5, 42.65]) - ]) - - self.parameters_updated = False - - self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) - self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - - self.LQR_controller = LQRController() - - self.update_controller_parameters() - - self.enabled = False - - 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("lqr_controller_node started") - - def update_controller_parameters(self): - Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value - R = self.get_parameter('lqr_controller.R').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 - - D = np.diag(D_diag) - M = np.reshape(M, (3, 3)) - - self.LQR_controller.update_parameters(M, D, Q, R) - - def parameter_callback(self, params): - self.parameters_updated = True - for param in params: - if param.name == 'lqr_controller.Q': - self.get_logger().info(f"Updated Q to {param.value}") - elif param.name == 'lqr_controller.R': - self.get_logger().info(f"Updated R to {param.value}") - elif param.name == 'physical.inertia_matrix': - self.get_logger().info(f"Updated inertia_matrix to {param.value}") - elif param.name == 'physical.damping_matrix_diag': - self.get_logger().info(f"Updated damping_matrix_diag to {param.value}") - - return SetParametersResult(successful=True) - - def state_cb(self, msg): - self.state = odometrymsg_to_state(msg) - - def guidance_cb(self, msg): - self.x_ref = odometrymsg_to_state(msg)[:3] - - def controller_callback(self): - if self.parameters_updated: - self.update_controller_parameters() - self.parameters_updated = False - - if hasattr(self, 'state') and hasattr(self, 'x_ref'): - self.LQR_controller.calculate_model(self.state[2]) - control_input = self.LQR_controller.calculate_control_input(self.state, self.x_ref) - wrench_msg = Wrench() - wrench_msg.force.x = control_input[0] - wrench_msg.force.y = control_input[1] - wrench_msg.torque.z = control_input[2] - self.wrench_publisher_.publish(wrench_msg) - -def main(args=None): - rclpy.init(args=args) - node = LQRControllerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml deleted file mode 100644 index 55cd3a82..00000000 --- a/motion/lqr_controller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - lqr_controller - 0.0.0 - This is an implementation of the LQR DP controller for the ASV - vortex - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - nav_msgs - geometry_msgs - vortex_msgs - std_msgs - numpy - matplotlib - scipy - - python3-pytest - - - ament_cmake - - diff --git a/motion/pid_controller/CMakeLists.txt b/motion/pid_controller/CMakeLists.txt deleted file mode 100644 index dffae1df..00000000 --- a/motion/pid_controller/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(pid_controller) - -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_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - pid_controller/pid_controller_node.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml deleted file mode 100644 index f600f9f0..00000000 --- a/motion/pid_controller/config/pid_config.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - pid_controller: - Kp: [130., 240., 60.] # Proportional costs for [x, y, heading] - Ki: [15., 29., 7.] # Integral costs for [x, y, heading] - Kd: [160., 300., 75.] # Derivative costs for [x, y, heading] \ No newline at end of file diff --git a/motion/pid_controller/launch/pid_controller.launch.py b/motion/pid_controller/launch/pid_controller.launch.py deleted file mode 100755 index 465924a9..00000000 --- a/motion/pid_controller/launch/pid_controller.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - pid_controller_node = Node( - package='pid_controller', - executable='pid_controller_node.py', - name='pid_controller_node', - parameters=[ - os.path.join(get_package_share_directory('pid_controller'),'config','pid_config.yaml'), - os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml') - ], - output='screen', - ) - return LaunchDescription([ - pid_controller_node - ]) diff --git a/motion/pid_controller/package.xml b/motion/pid_controller/package.xml deleted file mode 100644 index 1cb6be76..00000000 --- a/motion/pid_controller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - pid_controller - 0.0.0 - This is an implementation of the PID DP controller for the ASV - vortex - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - nav_msgs - geometry_msgs - vortex_msgs - std_msgs - numpy - matplotlib - scipy - - python3-pytest - - - ament_cmake - - diff --git a/motion/pid_controller/pid_controller/__init__.py b/motion/pid_controller/pid_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/pid_controller/pid_controller/conversions.py b/motion/pid_controller/pid_controller/conversions.py deleted file mode 100644 index 92f14cfb..00000000 --- a/motion/pid_controller/pid_controller/conversions.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np - -from transforms3d.euler import euler2quat, quat2euler -from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped - - -def odometrymsg_to_state(msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - -def state_to_odometrymsg(state): - orientation_list_next = euler2quat(0, 0, state[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = state[0] - odometry_msg.pose.pose.position.y = state[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_next[0] - odometry_msg.pose.pose.orientation.x = orientation_list_next[1] - odometry_msg.pose.pose.orientation.y = orientation_list_next[2] - odometry_msg.pose.pose.orientation.z = orientation_list_next[3] - - return odometry_msg - -def state_to_posestamped(state, frame_id, stamp): - orientation_list_next = euler2quat(0, 0, state[2]) - - posestamped_msg = PoseStamped() - - posestamped_msg.header.frame_id = frame_id - posestamped_msg.header.stamp = stamp - - posestamped_msg.pose.position.x = state[0] - posestamped_msg.pose.position.y = state[1] - posestamped_msg.pose.position.z = 0.0 - posestamped_msg.pose.orientation.w = orientation_list_next[0] - posestamped_msg.pose.orientation.x = orientation_list_next[1] - posestamped_msg.pose.orientation.y = orientation_list_next[2] - posestamped_msg.pose.orientation.z = orientation_list_next[3] - - return posestamped_msg - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py deleted file mode 100644 index 29624c66..00000000 --- a/motion/pid_controller/pid_controller/pid_controller.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np - -class PID: - def __init__(self): - self.error_sum = np.zeros(3) - - self.Kp = np.eye(3) - self.Ki = np.eye(3) - self.Kd = np.eye(3) - - self.upper = np.array([40, 40, 40]) - self.lower = np.array([-40, -40, -40]) - - # self.tau_max = np.array([100, 100, 100]) - - def update_parameters(self, Kp, Ki, Kd): - self.Kp = np.diag(Kp) - self.Ki = np.diag(Ki) - self.Kd = np.diag(Kd) - - def calculate_control_input(self, eta, eta_d, eta_dot, dt): - error = eta - eta_d - self.error_sum += error * dt - self.error_sum = np.clip(self.error_sum, -20, 20) - - p = self.Kp @ error - i = self.Ki @ self.error_sum - d = self.Kd @ eta_dot - - self.last_error = error - - tau = -(p + i + d) - - # if tau[0] > self.tau_max[0]: - # tau[0] = self.tau_max[0] - # elif tau[0] < -self.tau_max[0]: - # tau[0] = -self.tau_max[0] - - # if tau[1] > self.tau_max[1]: - # tau[1] = self.tau_max[1] - # elif tau[1] < -self.tau_max[1]: - # tau[1] = -self.tau_max[1] - - # if tau[2] > self.tau_max[2]: - # tau[2] = self.tau_max[2] - # elif tau[2] < -self.tau_max[2]: - # tau[2] = -self.tau_max[2] - - return tau - - @staticmethod - def cap_array(arr: np.ndarray, lower: np.ndarray, upper: np.ndarray): - new_arr = np.zeros(3) - for i in range(len(arr)): - if arr[i] > upper[i]: - new_arr[i] = upper[i] - elif arr[i] < lower[i]: - new_arr[i] = lower - else: - new_arr[i] = arr[i] diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py deleted file mode 100755 index a8927ea0..00000000 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from pid_controller import PID -from conversions import odometrymsg_to_state -from time import sleep -from rcl_interfaces.msg import SetParametersResult - -from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy - -qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, - reliability=QoSReliabilityPolicy.BEST_EFFORT) - -class PIDControllerNode(Node): - def __init__(self): - super().__init__("pid_controller_node") - - self.declare_parameters( - namespace='', - parameters=[ - ('pid_controller.Kp', [1.0, 1.0, 1.0]), - ('pid_controller.Ki', [1.0, 1.0, 1.0]), - ('pid_controller.Kd', [1.0, 1.0, 1.0]), - ]) - - self.parameters_updated = False - - self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) - self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1) - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - - ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) - # M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value - - # M = np.reshape(M, (3, 3)) - # M_diag = np.diag(M) - # omega_n = 1.2 - # zeta = 0.75 - # Kp = M_diag * omega_n**2 - # self.get_logger().info(f"Kp: {Kp}") - # Kd = M_diag * 2 * zeta * omega_n #- D_diag - # self.get_logger().info(f"Kd: {Kd}") - # Ki = omega_n/10 * Kp - # self.get_logger().info(f"Ki: {Ki}") - - self.pid_controller = PID() - - self.update_controller_parameters() - - self.enabled = False - - self.controller_period = 0.1 - self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback) - self.add_on_set_parameters_callback(self.parameter_callback) - - self.get_logger().info("pid_controller_node started") - - def update_controller_parameters(self): - self.pid_controller.error_sum = np.zeros(3) - - Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value - Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value - Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value - - self.pid_controller.update_parameters(Kp, Ki, Kd) - - self.get_logger().info("Updated controller parameters") - - def parameter_callback(self, params): - self.parameters_updated = True - for param in params: - if param.name == 'pid_controller.Kp': - self.get_logger().info(f"Updated Kp to {param.value}") - elif param.name == 'pid_controller.Ki': - self.get_logger().info(f"Updated Ki to {param.value}") - elif param.name == 'pid_controller.Kd': - self.get_logger().info(f"Updated Kd to {param.value}") - - return SetParametersResult(successful=True) - - def state_cb(self, msg): - self.state = odometrymsg_to_state(msg) - - def guidance_cb(self, msg): - self.x_ref = odometrymsg_to_state(msg)[:3] - - def controller_callback(self): - if hasattr(self, 'state') and hasattr(self, 'x_ref'): - control_input = self.pid_controller.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period) - # self.get_logger().info(f"Control input: {control_input}") - wrench_msg = Wrench() - wrench_msg.force.x = control_input[0] - wrench_msg.force.y = control_input[1] - wrench_msg.torque.z = control_input[2] - self.wrench_publisher_.publish(wrench_msg) - -def main(args=None): - rclpy.init(args=args) - node = PIDControllerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main()