diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt deleted file mode 100644 index 0f41d72e..00000000 --- a/mission/waypoint_manager/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(waypoint_manager) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) - -ament_python_install_package(scripts) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - scripts/waypoint_manager.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) -find_package(ament_cmake_pytest REQUIRED) -set(_pytest_tests - tests/test_waypoint_manager.py -) -foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 60 - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - ) -endforeach() -endif() - -ament_package() diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml deleted file mode 100644 index b5dde0d1..00000000 --- a/mission/waypoint_manager/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - waypoint_manager - 0.0.0 - Waypoint manager package - alice - MIT - - ament_cmake - - rclpy - geometry_msgs - ros2launch - rnav_msgs - vortex_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_pytest - - - ament_cmake - - diff --git a/mission/waypoint_manager/scripts/Waypoint2D.py b/mission/waypoint_manager/scripts/Waypoint2D.py deleted file mode 100644 index 2fd0d8a6..00000000 --- a/mission/waypoint_manager/scripts/Waypoint2D.py +++ /dev/null @@ -1,7 +0,0 @@ -import dataclasses - - -@dataclasses.dataclass(frozen=True) -class Waypoint2D: - north: float - east: float \ No newline at end of file diff --git a/mission/waypoint_manager/scripts/__init__.py b/mission/waypoint_manager/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/mission/waypoint_manager/scripts/waypoint_manager.py b/mission/waypoint_manager/scripts/waypoint_manager.py deleted file mode 100755 index 8714089a..00000000 --- a/mission/waypoint_manager/scripts/waypoint_manager.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from vortex_msgs.srv import Waypoint -from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped, Point -from scripts.Waypoint2D import Waypoint2D - - -class WaypointManager(Node): - """ - Nodes created: WaypointManager - Subscribes to: d_star_lite - Publishes to: hybridpath_guidance - """ - - def __init__(self): - """ - Initializes the WaypointManager node, sets up services, clients, and publishers. - """ - - super().__init__("WaypointManager") - - self.waypoint_list = [] - - self.waypoint_service = self.create_service(Waypoint, 'waypoint', self.waypoint_callback) - self.add_waypoint_service = self.create_service(Waypoint, 'add_waypoint', self.add_waypoint_callback) - self.waypoint_list_client = self.create_client(Waypoint, 'waypoint_list') - - self.path = Path() - self.path.header.frame_id = 'world' - - def waypoint_callback(self, request, response): - """ - Clears the current waypoint list and adds new waypoints from the request. - - Args: - request: The request message containing waypoints. - response: The response message indicating the success of the operation. - - Returns: - The response message with the success status. - """ - - self.waypoint_list.clear() - return self.add_waypoint_to_list(request) - - - def add_waypoint_to_list(self, req): - """ - Adds waypoints to the waypoint list from the request. - - This method processes a request containing a list of waypoints, converting them into `Waypoint2D` objects, - and appending them to the waypoint list. It also updates the `path` with new poses. - - Args: - req (Waypoint.Request): Request containing the list of waypoints to be added. - - Returns: - Waypoint.Response: A response object indicating success. - """ - - new_waypoints = list(zip(req.waypoint[::2], req.waypoint[1::2])) - self.waypoint_list.extend([Waypoint2D(north=n, east=e) for n, e in new_waypoints]) - - for waypoint in self.waypoint_list: - new_pose = PoseStamped() - new_pose.pose.position.x = waypoint.north - new_pose.pose.position.y = waypoint.east - new_pose.pose.position.z = 0.0 - self.path.poses.append(new_pose) - - response = Waypoint.Response() - response.success = True - return response - - def add_waypoint_callback(self, request, response): - """ - Callback function for the add_waypoint service. - - Forwards the request to `add_waypoint_to_list` method. - - Args: - request: The request object containing new waypoints. - response: The response object to be filled by `add_waypoint_to_list`. - - Returns: - The response from `add_waypoint_to_list`. - """ - - return self.add_waypoint_to_list(request) - - def sending_waypoints(self): - """ - Sends the current list of waypoints to the waypoint list service. - - Waits for the waypoint list service to become available. Once available, - it sends the current list of waypoints and waits for a response. Logs the - outcome of the service call. - """ - - while not self.waypoint_list_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Waypoint service not available, waiting again...') - request = Waypoint.Request() - - request.waypoints = self.waypoint_list - future = self.waypoint_list_client.call_async(request) - - rclpy.spin_until_future_complete(self, future, timeout_sec = 1.0) - try: - response = future.result() - self.get_logger().info(f'Waypoints successfully submitted: {response.success}') - except Exception as e: - self.get_logger().error('Service call failed %r' % (e,)) - -def main(args=None): - rclpy.init(args=args) - waypoint_manager = WaypointManager() - rclpy.spin(waypoint_manager) - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/mission/waypoint_manager/tests/__init__.py b/mission/waypoint_manager/tests/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/mission/waypoint_manager/tests/test_waypoint_manager.py b/mission/waypoint_manager/tests/test_waypoint_manager.py deleted file mode 100755 index 4a0d593e..00000000 --- a/mission/waypoint_manager/tests/test_waypoint_manager.py +++ /dev/null @@ -1,43 +0,0 @@ -import rclpy -import unittest -from scripts.waypoint_manager import WaypointManager -from vortex_msgs.srv import Waypoint -from scripts.Waypoint2D import Waypoint2D - -class TestWaypointManager(unittest.TestCase): - def setUp(self): - rclpy.init() - self.node = WaypointManager() - - def tearDown(self): - self.node.destroy_node() - rclpy.shutdown() - - def test_add_waypoint_to_list(self): - # Mocking a request object - class MockRequest1: - waypoint = [1.0, 2.0] - class MockRequest2: - waypoint = [2.0, 3.1] - - request1 = MockRequest1() - request2 = MockRequest2() - - # Call the method - response1 = self.node.add_waypoint_to_list(request1) - - # Check if the waypoint is added - self.assertTrue(response1.success) - self.assertIn(Waypoint2D(north =1.0, east=2.0), self.node.waypoint_list) - - # Check that a waypoint that has not been added is not in the list - self.assertNotIn(Waypoint2D(north =2.0, east=3.1), self.node.waypoint_list) - - # Adds and checks if the waypoint is added - response2 = self.node.add_waypoint_to_list(request2) - self.assertTrue(response2.success) - self.assertIn(Waypoint2D(north =2.0, east=3.1), self.node.waypoint_list) - - -if __name__ == '__main__': - unittest.main() diff --git a/motion/colav/scripts/VO_math.py b/motion/colav/scripts/VO_math.py index aadd1cda..c4de57e0 100644 --- a/motion/colav/scripts/VO_math.py +++ b/motion/colav/scripts/VO_math.py @@ -57,8 +57,8 @@ def set_cone_angles(self) -> None: """ Calculates the largest and smallest heading-angle where a collision can occur """ - theta_ro = math.atan2(self.vessel.y - self.obstacle.y, - self.vessel.x - self.obstacle.x) + theta_ro = math.atan2(self.obstacle.y - self.vessel.y, + self.obstacle.x - self.vessel.x) print("ob", self.vessel.radius, self.obstacle.radius) distance = np.linalg.norm([self.obstacle.x - self.vessel.x, self.obstacle.y - self.vessel.y]) @@ -79,10 +79,10 @@ def check_if_collision(self) -> bool: """ - buffer = 0 + buffer = math.pi / 12 dvx = self.vessel.vx - self.obstacle.vx dvy = self.vessel.vy - self.obstacle.vy angle = math.atan2(dvy, dvx) - print(f"VO left angle: {self.left_angle}, VO right angle: {self.right_angle}, Velocity angle: {angle}") + # print(f"VO left angle: {self.left_angle}, VO right angle: {self.right_angle}, Velocity angle: {angle}") return angle > self.right_angle - buffer and angle < self.left_angle + buffer \ No newline at end of file diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index d2e2f075..79658b22 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -103,7 +103,7 @@ def gen_colav_data(self): return self.create_guidance_data(0, 0, self.vessel.heading, self.vessel_odom) elif zone == Zones.COLIMM and not VO.check_if_collision(): self.get_logger().info(f'No collision detected!') - return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom, is_colav=True) + return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom) approach = self.gen_approach(closest_obst, self.vessel) self.get_logger().info(f'Collition Detected!!!!') @@ -111,7 +111,6 @@ def gen_colav_data(self): if approach in [Approaches.FRONT, Approaches.RIGHT]: buffer = math.pi / 6 # 30 degrees new_heading = VO.right_angle - buffer - #self.get_logger().info(f'New heading: {new_heading}, current heading: {self.vessel.heading}, VO right angle: {VO.right_angle}, buffer: {buffer}') return self.create_guidance_data(self.vessel.speed, new_heading, self.vessel.heading, self.vessel_odom) elif approach in [Approaches.BEHIND, Approaches.LEFT]: return None @@ -125,27 +124,19 @@ def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_col data.psi_d = float(psi_d) data.u = float(speed) # Assuming this is the desired speed data.t = float(self.get_clock().now().seconds_nanoseconds()[0]) + float(self.get_clock().now().seconds_nanoseconds()[1]) * 1e-9 - # orientation_q = Quaternion( - # x=vessel_odom.pose.pose.orientation.x, - # y=vessel_odom.pose.pose.orientation.y, - # z=vessel_odom.pose.pose.orientation.z, - # w=vessel_odom.pose.pose.orientation.w) - orientation_list = [ + + orientation_quat = [ vessel_odom.pose.pose.orientation.x, vessel_odom.pose.pose.orientation.y, vessel_odom.pose.pose.orientation.z, vessel_odom.pose.pose.orientation.w ] # _, _, yaw = self.quaternion_to_euler(orientation_q) - yaw = quat2euler(orientation_list)[2] + yaw = quat2euler(orientation_quat)[2] self.get_logger().info(f'Current yaw: {yaw}') data.psi = float(yaw) data.is_colav = is_colav return data - - def normalize_angle(self, angle): - """Normalize angle to be within the range [0, 2*pi)""" - return angle % (2 * math.pi) def gen_approach(self, obstacle: Obstacle, vessel: Obstacle): dx = obstacle.x - vessel.x @@ -155,9 +146,9 @@ def gen_approach(self, obstacle: Obstacle, vessel: Obstacle): if vessel.heading + buffer > phi > vessel.heading - buffer: return Approaches.FRONT - elif self.normalize_angle(math.pi - (vessel.heading - buffer)) < phi < self.normalize_angle(vessel.heading - buffer): + elif (math.pi + vessel.heading - buffer) > phi > (vessel.heading + buffer): return Approaches.RIGHT - elif self.normalize_angle(math.pi - (vessel.heading + buffer)) > phi > self.normalize_angle(vessel.heading + buffer): + elif (vessel.heading - math.pi + buffer) < phi < (vessel.heading - buffer): return Approaches.LEFT return Approaches.BEHIND diff --git a/motion/colav/scripts/colav_sim.py b/motion/colav/scripts/colav_sim.py index 1453dc6a..ff070979 100755 --- a/motion/colav/scripts/colav_sim.py +++ b/motion/colav/scripts/colav_sim.py @@ -21,11 +21,11 @@ def __init__(self): self.pos_y = 0 self.speed = 0.5 self.obstacle_pos_x = 5 - self.obstacle_pos_y = 5 + self.obstacle_pos_y = 0 self.obstacle_speed = 0.5 - self.obstacle_heading = 5*np.pi/4 + self.obstacle_heading = 4*np.pi/4 self.goal_pos_x = 8 - self.goal_pos_y = 8 + self.goal_pos_y = 0 self.dt = 0.1