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