diff --git a/src/action/stanley_controller/launch/stanley_controller_launch.py b/src/action/stanley_controller/launch/stanley_controller_launch.py new file mode 100644 index 00000000..8d54adf1 --- /dev/null +++ b/src/action/stanley_controller/launch/stanley_controller_launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + stanley_controller = Node( + package='stanley_controller', + executable='stanley_controller.py', + name='stanley_controller', + output='screen' + ) + + + return LaunchDescription([ + stanley_controller + ]) \ No newline at end of file diff --git a/src/control/controller/LICENSE b/src/control/controller/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/control/controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/control/controller/controller/__init__.py b/src/control/controller/controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/control/controller/controller/head_to_goal_controller.py b/src/control/controller/controller/head_to_goal_controller.py new file mode 100644 index 00000000..b1318790 --- /dev/null +++ b/src/control/controller/controller/head_to_goal_controller.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python3 +# Python imports +from typing import Optional +import rclpy +from rclpy.node import Node +import numpy as np +import math +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import Pose +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + + +class head_to_goal_control_algorithm(Node): + def __init__(self): + super().__init__("Head_To_Goal_Controller") + self.get_logger().info("Head to Goal Controller Node v5 Started") + + # Constant to tune (touch me please it makes me feel horny ahhhhhhh!) + ## Tuning for look ahead distance + self.look_up_distance = 1 + self.cancel_distance = 0.7 + ## Tuning for PID controller + self.P = 200 + self.max_steering_angle = 20.0 + #self.max_speed = 2.5 + self.max_speed = 3.0 + self.speed_adjuster_width = 15 + ## Current speed setting + self.current_speed = 0 + + # Initializer (normally don't touch) + self.steering_angle = 0 + self.pos = (0,0) + self.speed_decay_constant = 0 + + # subscribe to best trajectory + self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + #self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + self.car_pos_sub = self.create_subscription(Pose, "car_position", self.main_hearback, 5) + + self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + + self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5) + + def main_hearback(self, msg: Pose): + # Update car's current location and update transformation matrix + #self.car_pose = msg.cones[0].pose.pose + self.car_pose = msg + self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) + + # Before proceed, check whether we have a trajectory input + if hasattr(self, "trajectory_in_global_frame"): + # Update destination point to track + self.update_track_point(self.trajectory_in_global_frame) + # Get expected steering angle to publish + self.steering_angle = self.get_steering_angle(self.Pose_to_track_in_global_frame) + self.steering_angle = self.saturating_steering(self.steering_angle) + # self.get_logger().info(f"Set steering angle to {self.steering_angle * self.P}") + + else: + self.steering_angle = 0 + #self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + + # Experimental: speed adjuster + # self.current_speed = self.steer_to_speed(self.steering_angle) + # Adjust the speed base on the performance of the path planning (if takes long time to locate next destination then stop until refresh) + self.apply_speed_decay() + + # Publish command for velocity + self.publish_ackermann() + + def selected_trajectory_handler(self, msg: PoseArray): + self.trajectory_in_global_frame = msg + + def saturating_steering(self, steering_angle): + saturation = self.max_steering_angle + if steering_angle > saturation: + steering_angle = saturation + elif steering_angle < -1 * saturation: + steering_angle = -1 * saturation + + return steering_angle + + def steer_to_speed(self, steering_angle): + # RF with NN can be used here + speed = self.max_speed * np.exp(- (steering_angle ** 2) / (2 * (self.speed_adjuster_width ** 2))) + return speed + + def apply_speed_decay(self): + self.current_speed = (0.61 ** self.speed_decay_constant) * self.max_speed + #self.get_logger().info(f"Speed decay applied: {self.speed_decay_constant}, set current speed to {self.current_speed}") + + # Coordinate tranformer + def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( + np.array, np.array, np.array): + '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones + + Args: + x: x position specified in float + y: y position specified in float + theta: theta orientation specified in float + list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) + + Returns: + position_vector: np.array of position vector of cart + rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame + list_of_cones: np.array 2 x n matrix of the list of cones measured in global frame + + Raises: + None + ''' + position_vector = np.array([[x], [y]]) + rotation_matrix_l2g = np.array( + [[math.cos(theta), -math.sin(theta)], + [math.sin(theta), math.cos(theta)]]) # local coordinate to global coordinate matrix + rotation_matrix_g2l = np.array( + [[math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)]]) # global coordinate to local coordinate matrix + + return position_vector, rotation_matrix_l2g, rotation_matrix_g2l + + + def get_track_point_in_local_frame(self, track_point_in_global_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_local_frame = Pose() + position_input = np.array( + [[track_point_in_global_frame.position.x], [track_point_in_global_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_g2l, (position_input - self.position_vector)) + track_point_in_local_frame.position.x = float(position_output[0]) + track_point_in_local_frame.position.y = float(position_output[1]) + return track_point_in_local_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_global_frame + + + def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_global_frame = Pose() + position_input = np.array( + [[track_point_in_local_frame.position.x], [track_point_in_local_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_l2g, position_input) + self.position_vector + track_point_in_global_frame.position.x = float(position_output[0]) + track_point_in_global_frame.position.y = float(position_output[1]) + return track_point_in_global_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_local_frame + +# Picking and maintaining a track point + def update_track_point(self, msg: PoseArray): #Main logic + # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible + if self.need_new_track_point(): + #self.get_logger().info("Updating track point, speed decay applied") + self.Pose_to_track_in_global_frame = self.get_track_point_in_global_frame(msg) + self.speed_decay_constant += 1 + else: + self.speed_decay_constant = 0 + self.track_point_pub.publish(self.Pose_to_track_in_global_frame) + + def need_new_track_point(self): + if not(hasattr(self, "Pose_to_track_in_global_frame")): + return True + else: + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame) + if self.track_point_still_visible(Pose_to_track_in_local_frame): + if Pose_to_track_in_local_frame.position.y <= self.cancel_distance: + return True + else: + return False + else: + return True + + def get_track_point_in_global_frame(self, trajectory_in_global_frame: PoseArray): + if hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l"): + sorted_trajectory_poses_in_global_frame = PoseArray() + sorted_trajectory_poses_in_global_frame.poses = sorted(trajectory_in_global_frame.poses, key=lambda item: self.get_distance(self.get_track_point_in_local_frame(item))) + for track_point_in_global_frame in sorted_trajectory_poses_in_global_frame.poses: + track_point_in_local_frame = self.get_track_point_in_local_frame(track_point_in_global_frame) + if self.track_point_distance_above_look_up_distance(track_point_in_local_frame): + return track_point_in_global_frame + return self.car_pose + + def track_point_still_visible(self, track_point_in_local_frame: Pose): + return track_point_in_local_frame.position.y >= 0 + + def track_point_distance_above_look_up_distance(self, track_point_in_local_frame: Pose): + return self.get_distance(track_point_in_local_frame) >= self.look_up_distance + + def get_distance(self, track_point_in_local_frame: Pose): + x = track_point_in_local_frame.position.x + y = track_point_in_local_frame.position.y + return abs(((x ** 2) + (y ** 2)) ** (1/2)) + +### Calculate Steering Angle + def get_steering_angle(self, Pose_to_track_in_global_frame: Pose): + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(Pose_to_track_in_global_frame); + error_in_x = Pose_to_track_in_local_frame.position.x + return error_in_x * self.P + + def lateral_distance(self): + return self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame).position.x + + def get_arc_radius(self, L, x): + return L ** 2 / (2 * abs(x)) + + def publish_ackermann(self): + + args1 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + #print(msg1) + + args2 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg2 = AckermannDrive(**args2) + + msg_cmd_vel = self.convert_to_stamped(msg1) + self.cmd_vel_pub.publish(msg_cmd_vel) + self.drive_pub.publish(msg1) + self.drive_vis_pub.publish(msg2) + + def convert_to_stamped(self, ackermann_msgs): + stamped_msg = AckermannDriveStamped() + stamped_msg.header = Header() + stamped_msg.header.stamp = self.get_clock().now().to_msg() # Set the current timestamp + stamped_msg.header.frame_id = '0' # Set the appropriate frame ID + stamped_msg.drive = ackermann_msgs + return stamped_msg + +def main(args=None): + rclpy.init(args=args) + + pure_pursuiter = head_to_goal_control_algorithm() + + rclpy.spin(pure_pursuiter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pure_pursuiter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/control/controller/controller/pure_pursuit_controller.py b/src/control/controller/controller/pure_pursuit_controller.py new file mode 100644 index 00000000..91c6aa79 --- /dev/null +++ b/src/control/controller/controller/pure_pursuit_controller.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +# Python imports +from typing import Optional +import rclpy +from rclpy.node import Node +import numpy as np +import math +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import Pose +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive + + + +class pure_pursuit_algorithm(Node): + def __init__(self): + super().__init__("Pure_Pursuit_Controller") + self.get_logger().info("Pure Pursuit Node Started") + + # Constant to tune (touch me please it makes me feel horny ahhhhhhh!) + ## Tuning for look ahead distance + self.look_up_distance = 10 + ## Tuning for matching steering in theory with steering in actual + self.P = 100 + ## Current speed setting + self.current_speed = 2 + + # Initializer (normally don't touch) + self.steering_angle = 0 + self.pos = (0,0) + + # subscribe to best trajectory + self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5) + self.create_subscription(Pose, "car_position", self.get_car_position, 5) + self.cmd_vel_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5) + + def main_hearback(self, msg: ConeMap): + # Update car's current location and update transformation matrix + self.car_pose = self.car_position_pose + self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w) + + # Before proceed, check whether we have a trajectory input + if hasattr(self, "trajectory_in_global_frame"): + # Update destination point to track + self.update_track_point(self.trajectory_in_global_frame) + + # Get expected steering angle to publish + self.steering_angle = self.get_steering_angle(self.Pose_to_track_in_global_frame) + + self.steering_angle = self.saturating_steering(self.steering_angle) + # self.get_logger().info(f"Set steering angle to {self.steering_angle * self.P}") + + else: + self.steering_angle = 0 + self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + + # Publish command for velocity + self.publish_ackermann() + + def selected_trajectory_handler(self, msg: PoseArray): + self.trajectory_in_global_frame = msg + + def saturating_steering(self, steering_angle): + saturation = 10 + if steering_angle > saturation: + steering_angle = saturation + elif steering_angle < -1 * saturation: + steering_angle = -1 * saturation + + return steering_angle + + + # Coordinate tranformer + def convert_to_transformation_matrix(self, x: float, y: float, theta: float) -> ( + np.array, np.array, np.array): + '''Convert state and list_of_cones input into position vector, rotation matrix (DCM) and the matrix of list of cones + + Args: + x: x position specified in float + y: y position specified in float + theta: theta orientation specified in float + list_of_cones: np.array (numpy array) for matrix of cone positions in 2 by n matrix (n is number of cones recorded in input)) + + Returns: + position_vector: np.array of position vector of cart + rotation_matrix: np.array DCM matrix to convert reading from local frame into global frame + list_of_cones: np.array 2 x n matrix of the list of cones measured in global frame + + Raises: + None + ''' + position_vector = np.array([[x], [y]]) + rotation_matrix_l2g = np.array( + [[math.cos(theta), -math.sin(theta)], + [math.sin(theta), math.cos(theta)]]) # local coordinate to global coordinate matrix + rotation_matrix_g2l = np.array( + [[math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)]]) # global coordinate to local coordinate matrix + + return position_vector, rotation_matrix_l2g, rotation_matrix_g2l + + + def get_track_point_in_local_frame(self, track_point_in_global_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_local_frame = Pose() + position_input = np.array( + [[track_point_in_global_frame.position.x], [track_point_in_global_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_g2l, (position_input - self.position_vector)) + track_point_in_local_frame.position.x = float(position_output[0]) + track_point_in_local_frame.position.y = float(position_output[1]) + return track_point_in_local_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_global_frame + + + def get_track_point_in_global_frame(self, track_point_in_local_frame: Pose): + if (hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l")): + track_point_in_global_frame = Pose() + position_input = np.array( + [[track_point_in_local_frame.position.x], [track_point_in_local_frame.position.y]]) + position_output = np.matmul(self.rotation_matrix_l2g, position_input) + self.position_vector + track_point_in_global_frame.position.x = float(position_output[0]) + track_point_in_global_frame.position.y = float(position_output[1]) + return track_point_in_global_frame + else: + self.get_logger.info("Warning: Local pose message not transformed to global frame") + return track_point_in_local_frame + +# Picking and maintaining a track point + def update_track_point(self, msg: PoseArray): #Main logic + # Pick new tracking point if no tracking point is selected or old tracking point is no longer visible + if self.need_new_track_point(): + self.Pose_to_track_in_global_frame = self.get_track_point_in_global_frame(msg) + self.track_point_pub.publish(self.Pose_to_track_in_global_frame) + + def need_new_track_point(self): + if not(hasattr(self, "Pose_to_track_in_global_frame")): + return True + else: + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame) + if self.track_point_still_visible(Pose_to_track_in_local_frame): + return False + else: + return True + + def get_track_point_in_global_frame(self, trajectory_in_global_frame: PoseArray): + if hasattr(self, "position_vector") and hasattr(self, "rotation_matrix_l2g") and hasattr(self, "rotation_matrix_g2l"): + sorted_trajectory_poses_in_global_frame = PoseArray() + sorted_trajectory_poses_in_global_frame.poses = sorted(trajectory_in_global_frame.poses, key=lambda item: self.get_distance(self.get_track_point_in_local_frame(item))) + for track_point_in_global_frame in sorted_trajectory_poses_in_global_frame.poses: + track_point_in_local_frame = self.get_track_point_in_local_frame(track_point_in_global_frame) + if self.track_point_distance_above_look_up_distance(track_point_in_local_frame): + return track_point_in_global_frame + return self.car_pose + + def track_point_still_visible(self, track_point_in_local_frame: Pose): + return track_point_in_local_frame.position.y >= 0 + + def track_point_distance_above_look_up_distance(self, track_point_in_local_frame: Pose): + return self.get_distance(track_point_in_local_frame) >= self.look_up_distance + + def get_distance(self, track_point_in_local_frame: Pose): + x = track_point_in_local_frame.position.x + y = track_point_in_local_frame.position.y + return abs(((x ** 2) + (y ** 2)) ** (1/2)) + +### Calculate Steering Angle + def get_steering_angle(self, Pose_to_track_in_global_frame: Pose): + Pose_to_track_in_local_frame = self.get_track_point_in_local_frame(Pose_to_track_in_global_frame); + L = self.get_distance(Pose_to_track_in_local_frame) # Length from origin + x = Pose_to_track_in_local_frame.position.x + arc_radius = self.get_arc_radius(L,x) + steering_angle = 1 / arc_radius + if Pose_to_track_in_local_frame.position.x > 0: + return abs(steering_angle) + elif Pose_to_track_in_local_frame.position.x < 0: + return -1 * abs(steering_angle) + else: + return 0.0 + + def lateral_distance(self): + return self.get_track_point_in_local_frame(self.Pose_to_track_in_global_frame).position.x + + def get_arc_radius(self, L, x): + return L ** 2 / (2 * abs(x)) + + def publish_ackermann(self): + + args1 = {"steering_angle": float(self.steering_angle * self.P), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + print(msg1) + + args2 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg2 = AckermannDrive(**args2) + self.cmd_vel_pub.publish(msg1) + self.cmd_vis_pub.publish(msg2) + + def get_car_position(self, msg:Pose): self.car_position_pose = msg + +def main(args=None): + rclpy.init(args=args) + + pure_pursuiter = pure_pursuit_algorithm() + + rclpy.spin(pure_pursuiter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + pure_pursuiter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/control/controller/controller/stanley_controller.py b/src/control/controller/controller/stanley_controller.py new file mode 100644 index 00000000..d0b274f9 --- /dev/null +++ b/src/control/controller/controller/stanley_controller.py @@ -0,0 +1,209 @@ +import rclpy +from rclpy.node import Node +import numpy as np +import math +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import Pose +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + +def angle_mod(x, zero_2_2pi=False, degree=False): + """ + Angle modulo operation + Default angle modulo range is [-pi, pi) + + Parameters + ---------- + x : float or array_like + A angle or an array of angles. This array is flattened for + the calculation. When an angle is provided, a float angle is returned. + zero_2_2pi : bool, optional + Change angle modulo range to [0, 2pi) + Default is False. + degree : bool, optional + If True, then the given angles are assumed to be in degrees. + Default is False. + + Returns + ------- + ret : float or ndarray + an angle or an array of modulated angle. + + Examples + -------- + >>> angle_mod(-4.0) + 2.28318531 + + >>> angle_mod([-4.0]) + np.array(2.28318531) + + >>> angle_mod([-150.0, 190.0, 350], degree=True) + array([-150., -170., -10.]) + + >>> angle_mod(-60.0, zero_2_2pi=True, degree=True) + array([300.]) + + """ + if isinstance(x, float): + is_float = True + else: + is_float = False + + x = np.asarray(x).flatten() + if degree: + x = np.deg2rad(x) + + if zero_2_2pi: + mod_angle = x % (2 * np.pi) + else: + mod_angle = (x + np.pi) % (2 * np.pi) - np.pi + + if degree: + mod_angle = np.rad2deg(mod_angle) + + if is_float: + return mod_angle.item() + else: + return mod_angle + +class StanleyControl(Node): + + def __init__(self): + super().__init__('Stanley_Controller') + self.get_logger().info("Stanley Controller Node Started") + + #Constants + self.k_stanley = 5.0 #stanley Controller gain + self.k_speed = 1.0 #speed Controller gain + self.cam_fron_axle_dist= 1 #[m] Wheel base of vehicle + self.max_steer = 27.0 # [degrees] max steering angle + self.target_speed = 3.6/3.6 #[m/s] + + #Subscribe for car pose and track + self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5) + self.create_subscription(Pose, "car_position", self.main_hearback, 5) + #Publish result + self.cmd_drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "/cmd_vel", 5) + self.create_publisher(Pose, "moa/track_point", 5) + + def main_hearback(self, msg): + car_pose = msg + camera_position = [car_pose.position.x,car_pose.position.y] + car_yaw = car_pose.orientation.w + self.car_yaw_corrected = self.normalize_angle(car_yaw-4.71) + + + if hasattr(self, "trajectory_in_global_frame"): + #Get Car front axle center position + axle_pos = self.get_front_axle_position(camera_position,self.car_yaw_corrected) + #Get closest point on track and distance error + cls_point,error_front_axle = self.get_closest_track_point(axle_pos,self.car_yaw_corrected) + #Compute target yaw - Angle from positive x in radians + target_yaw = self.cal_target_yaw(cls_point) + #Compute steering angle + theta_e = -(target_yaw-self.car_yaw_corrected) + theta_d = -np.arctan2(self.k_stanley * error_front_axle, self.target_speed) + delta = math.degrees(theta_e + theta_d) + delta = np.clip(delta, -self.max_steer, self.max_steer) + self.steering_angle = delta + self.target_speed = 3.6/3.6 + else: + self.steering_angle = 0 + self.target_speed = 0 + self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!") + + # Publish command for velocity + self.publish_ackermann() + + def selected_trajectory_handler(self, msg): + self.trajectory_in_global_frame = msg + self.tx = [] + self.ty = [] + for tp in self.trajectory_in_global_frame.poses: + self.tx.append(tp.position.x) + self.ty.append(tp.position.y) + + def publish_ackermann(self): + + args1 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.target_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + + args2 = {"steering_angle": float(self.steering_angle), + "steering_angle_velocity": 0.0, + "speed": float(self.target_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg2 = AckermannDrive(**args2) + + args3 = {"header": Header(stamp=self.get_clock().now().to_msg(),frame_id="stanley_controller"), + "drive": msg1} + msg3 = AckermannDriveStamped(**args3) + + self.cmd_drive_pub.publish(msg1) + self.cmd_vis_pub.publish(msg2) + #self.cmd_vel_pub.publish(msg3) + + + def get_front_axle_position(self,cam_pos,car_yaw): + axle_pos = [cam_pos[0]+self.cam_fron_axle_dist*np.cos(car_yaw),cam_pos[1]+self.cam_fron_axle_dist*np.sin(car_yaw)] + return axle_pos + + def get_closest_track_point(self,axle_pos,car_yaw): + dx = list(axle_pos[0]-np.asarray(self.tx)) + dy = list(axle_pos[1]-np.asarray(self.ty)) + d = np.hypot(dx, dy) + target_idx = np.argmin(d) + + # Project RMS error onto front axle vector + front_axle_vec = [-np.cos(car_yaw + np.pi / 2),-np.sin(car_yaw + np.pi / 2)] + error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec) + return target_idx, error_front_axle + + def cal_target_yaw(self,cls_point): + if(cls_point==(len(self.ty)-1)): + dy = self.ty[cls_point]-self.ty[cls_point-1] + dx = self.tx[cls_point]-self.tx[cls_point-1] + else: + dy = self.ty[cls_point+1]-self.ty[cls_point] + dx = self.tx[cls_point+1]-self.tx[cls_point] + + target_yaw_op1 = self.normalize_angle(np.arctan2(dy,dx)) + target_yaw_op2 = self.normalize_angle(target_yaw_op1 + np.pi) + + yaw_diff_1 = abs(target_yaw_op1-self.car_yaw_corrected) + yaw_diff_2 = abs(target_yaw_op2-self.car_yaw_corrected) + target_yaw2 = target_yaw_op1 if yaw_diff_1 + + + controller + 0.0.0 + This package contains all the control nodes in the control pipeline- different from the hardware controllers found in the moa hierarchy folder + sivasriram16 + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/control/controller/resource/controller b/src/control/controller/resource/controller new file mode 100644 index 00000000..e69de29b diff --git a/src/control/controller/setup.cfg b/src/control/controller/setup.cfg new file mode 100644 index 00000000..1049f8af --- /dev/null +++ b/src/control/controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/controller +[install] +install_scripts=$base/lib/controller diff --git a/src/control/controller/setup.py b/src/control/controller/setup.py new file mode 100644 index 00000000..755703ad --- /dev/null +++ b/src/control/controller/setup.py @@ -0,0 +1,33 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'controller' + +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']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sivasriram16', + maintainer_email='ssri357@aucklanduni.ac.nz', + description='ROS 2 Controller package with multiple controllers.', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'stanley = controller.stanley_controller:main', + 'head_to_goal = controller.head_to_goal_controller.main', + 'pure_pursuit = controller.pure_pursuit_controller.main', + ], + }, + data_files=[ + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], +) diff --git a/src/control/controller/test/test_copyright.py b/src/control/controller/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/control/controller/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/control/controller/test/test_flake8.py b/src/control/controller/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/control/controller/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/control/controller/test/test_pep257.py b/src/control/controller/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/control/controller/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'