From f949a2d4a6e3fe69b21407ad6bd02c9bb421de1f Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Wed, 3 Apr 2024 20:09:38 +0200 Subject: [PATCH] Removed los_guidance --- guidance/los_guidance/LICENSE | 17 ---- .../config/los_guidance_config.yaml | 6 -- .../launch/los_guidance.launch.py | 16 ---- .../los_guidance/los_guidance/__init__.py | 0 .../los_guidance/los_guidance/los_guidance.py | 49 ----------- .../los_guidance/los_guidance_node.py | 87 ------------------- guidance/los_guidance/package.xml | 22 ----- guidance/los_guidance/resource/los_guidance | 0 guidance/los_guidance/setup.cfg | 4 - guidance/los_guidance/setup.py | 30 ------- 10 files changed, 231 deletions(-) delete mode 100644 guidance/los_guidance/LICENSE delete mode 100644 guidance/los_guidance/config/los_guidance_config.yaml delete mode 100644 guidance/los_guidance/launch/los_guidance.launch.py delete mode 100644 guidance/los_guidance/los_guidance/__init__.py delete mode 100644 guidance/los_guidance/los_guidance/los_guidance.py delete mode 100644 guidance/los_guidance/los_guidance/los_guidance_node.py delete mode 100644 guidance/los_guidance/package.xml delete mode 100644 guidance/los_guidance/resource/los_guidance delete mode 100644 guidance/los_guidance/setup.cfg delete mode 100644 guidance/los_guidance/setup.py diff --git a/guidance/los_guidance/LICENSE b/guidance/los_guidance/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/guidance/los_guidance/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/guidance/los_guidance/config/los_guidance_config.yaml b/guidance/los_guidance/config/los_guidance_config.yaml deleted file mode 100644 index 13a0ada6..00000000 --- a/guidance/los_guidance/config/los_guidance_config.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - los_guidance: - p0: [0.0, 0.0] - p1: [20.0, 20.0] - look_ahead_distance: 5.0 \ No newline at end of file diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py deleted file mode 100644 index f6875f8e..00000000 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ /dev/null @@ -1,16 +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(): - los_guidance_node = Node( - package='los_guidance', - executable='los_guidance_node', - name='los_guidance_node', - parameters=[os.path.join(get_package_share_directory('los_guidance'),'config','los_guidance_config.yaml')], - output='screen', - ) - return LaunchDescription([ - los_guidance_node - ]) diff --git a/guidance/los_guidance/los_guidance/__init__.py b/guidance/los_guidance/los_guidance/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py deleted file mode 100644 index b8855d51..00000000 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ /dev/null @@ -1,49 +0,0 @@ -import numpy as np - -class LOSGuidance: - def __init__(self, p0: list[float], p1: list[float], p_next: list[float]): - self.set_path(p0, p1) - self.heading_ref = 50*np.pi/180 # magic init number!!! - self.p_next = [np.array([50, -40]), np.array([20.0, -80.0]), np.array([120.0, -60.0]), np.array([160, 0.0]), np.array([60.0, 60.0])] - self.acceptance_radius = 0.5 - - def set_path(self, p0: list[float], p1: list[float]): - self.p0 = np.array(p0) - self.p1 = np.array(p1) - - def calculate_R_Pi_p(self): - self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0]) - self.R_Pi_p = np.array( - [[np.cos(self.Pi_p), -np.sin(self.Pi_p)], - [np.sin(self.Pi_p), np.cos(self.Pi_p)]] - ) - - def calculate_distance(self, p0, p1): - return np.sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2) - - def switch_path(self): - self.p0, self.p1 = self.p1, self.p_next[0] - - def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: - self.set_path(self.p0, self.p1) - self.calculate_R_Pi_p() - p_asv = np.array([x[0], x[1]]) - errors = self.R_Pi_p.T @ (p_asv - self.p0) - along_track_error = errors[0] - - # Switch points to next pair if crossed orthogonal line or entered circle of acceptance - if ((along_track_error > 0.8*self.calculate_distance(self.p0, self.p1))): - self.switch_path() - self.calculate_R_Pi_p() - errors = self.R_Pi_p.T @ (p_asv - self.p0) - along_track_error = errors[0] - self.p_next.pop(0) - if (self.p_next == []): - self.p_next = [np.array([100000.0, 1000000.0])] - - - p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 - x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref]) - - return x_ref - diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py deleted file mode 100644 index 0341bb3c..00000000 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ /dev/null @@ -1,87 +0,0 @@ -import rclpy -import numpy as np -from rclpy.node import Node -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler, euler2quat -from geometry_msgs.msg import Point -from los_guidance.los_guidance import LOSGuidance - -class LOSGuidanceNode(Node): - def __init__(self): - super().__init__("los_guidance_node") - - self.declare_parameters( - namespace='', - parameters=[ - ('los_guidance.p0', [0.0, 0.0]), - ('los_guidance.p1', [0.0, 0.0]), - ('los_guidance.look_ahead_distance', 0.0) - ]) - - p0 = self.get_parameter('los_guidance.p0').get_parameter_value().double_array_value - p1 = self.get_parameter('los_guidance.p1').get_parameter_value().double_array_value - self.look_ahead = self.get_parameter('los_guidance.look_ahead_distance').get_parameter_value().double_value - - self.p_next = np.array([10000.0, 10000.0]) - - self.get_logger().info(f"p0: {p0}") - self.get_logger().info(f"p1: {p1}") - self.get_logger().info(f"look_ahead_distance: {self.look_ahead}") - - self.los_guidance = LOSGuidance(p0, p1, self.p_next) - - self.guidance_publisher_ = self.create_publisher(Odometry, "controller/lqr/reference", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) - self.point_subscriber_ = self.create_subscription(Point, "los/point", self.point_cb, 1) - - self.get_logger().info("los_guidance_node started") - - def state_cb(self, msg): - # self.get_logger().info("state_callback") - - 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 - (roll, pitch, yaw) = quat2euler(orientation_list) - - 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]) - - x_ref = self.los_guidance.calculate_LOS_x_ref(state, self.look_ahead) - - orientation_list_ref = euler2quat(roll, pitch, x_ref[2]) - - odometry_msg = Odometry() - odometry_msg.pose.pose.position.x = x_ref[0] - odometry_msg.pose.pose.position.y = x_ref[1] - odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.w = orientation_list_ref[0] - odometry_msg.pose.pose.orientation.x = orientation_list_ref[1] - odometry_msg.pose.pose.orientation.y = orientation_list_ref[2] - odometry_msg.pose.pose.orientation.z = orientation_list_ref[3] - - self.guidance_publisher_.publish(odometry_msg) - - #rate = rclpy.Rate(100) - - def point_cb(self, msg): - self.p_next.append(np.array([msg.x, msg.y])) - - -def main(args=None): - rclpy.init(args=args) - node = LOSGuidanceNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml deleted file mode 100644 index 023e7271..00000000 --- a/guidance/los_guidance/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - los_guidance - 0.0.0 - TODO: Package description - vortex - MIT - - rclpy - nav_msgs - python-transforms3d-pip - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/guidance/los_guidance/resource/los_guidance b/guidance/los_guidance/resource/los_guidance deleted file mode 100644 index e69de29b..00000000 diff --git a/guidance/los_guidance/setup.cfg b/guidance/los_guidance/setup.cfg deleted file mode 100644 index 8709d00f..00000000 --- a/guidance/los_guidance/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/los_guidance -[install] -install_scripts=$base/lib/los_guidance diff --git a/guidance/los_guidance/setup.py b/guidance/los_guidance/setup.py deleted file mode 100644 index cef8ea5a..00000000 --- a/guidance/los_guidance/setup.py +++ /dev/null @@ -1,30 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'los_guidance' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='vortex', - maintainer_email='vortex.git@vortexntnu.no', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'los_guidance_node = los_guidance.los_guidance_node:main' - ], - }, -)