From 1ea6ff910c24804c631d364a778051eb35c132c5 Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Wed, 3 Apr 2024 10:30:26 -0500 Subject: [PATCH 01/22] Setting up base --- src/control/pure_pursuit/pure_pursuit_node.py | 202 ++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100644 src/control/pure_pursuit/pure_pursuit_node.py diff --git a/src/control/pure_pursuit/pure_pursuit_node.py b/src/control/pure_pursuit/pure_pursuit_node.py new file mode 100644 index 00000000..dac303b1 --- /dev/null +++ b/src/control/pure_pursuit/pure_pursuit_node.py @@ -0,0 +1,202 @@ +# Imports +import os +import time +import math +import rospy2 +import rospkg +import numpy as np +from numpy import linalg as la +import matplotlib.pyplot as plt +from matplotlib import patches +from race.msg import drive_param +from rospkg import RosPack +from nav_msgs.msg import Odometry +from visualization_msgs.msg import Marker +from std_msgs.msg import Header, ColorRGBA +from tf.transformations import euler_from_quaternion, quaternion_from_euler +from geometry_msgs.msg import PoseStamped, Point, Twist, Quaternion, Pose, Point, Vector3 + + +# GLOBAL VARIABLES +xc = 0.0 +yc = 0.0 +yaw = 0.0 +vel = 0.0 +idx = 0 +waypoints = [] +v_prev_error = 0.0 +freqs = 10 + +# CAR VARIABLES +# LOOKAHEAD = 1.5 # 1.5 +# WB = 0.3421 +LOOKAHEAD = 0.2 # 1.5 +WB = 0.04 + +# PROGRAM VARIABLES +pure_pursuit_flag = True +show_animation = True + +def read_points(): + """ + CHANGE THIS PATH TO WHERE YOU HAVE SAVED YOUR CSV FILES + """ + r = rospkg.RosPack() + package_path = r.get_path('pure_pursuit') + file_name = 'wp_file.csv' #'racecar_walker.csv' + file_path = package_path + '/waypoints/' + file_name + with open(file_path) as f: + path_points = np.loadtxt(file_path, delimiter = ',') + return path_points + +def pose_callback(data): + """ + Get current state of the vehicle + """ + global xc, yc, yaw, vel + xc = data.pose.pose.position.x + yc = data.pose.pose.position.y + + # Convert Quaternions to Eulers + qx = data.pose.pose.orientation.x + qy = data.pose.pose.orientation.y + qz = data.pose.pose.orientation.z + qw = data.pose.pose.orientation.w + quaternion = (qx,qy,qz,qw) + euler = euler_from_quaternion(quaternion) + yaw = euler[2] + vel = la.norm(np.array([data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z]),2) + +def find_distance(x1, y1): + global xc, yc, yaw, waypoints + distance = math.sqrt((x1 - xc) ** 2 + (y1 - yc) ** 2) + return distance + +def find_distance_index_based(idx): + global xc, yc, yaw, waypoints + x1 = float(waypoints[idx][0]) + y1 = float(waypoints[idx][1]) + distance = math.sqrt((x1 - xc) ** 2 + (y1 - yc) ** 2) + return distance + +def find_nearest_waypoint(): + """ + Get closest idx to the vehicle + """ + global xc, yc, yaw, waypoints, WB + curr_xy = np.array([xc, yc]) + waypoints_xy = waypoints[:, :2] + nearest_idx = np.argmin(np.sum((curr_xy - waypoints_xy)**2, axis=1)) + return nearest_idx + +def idx_close_to_lookahead(idx): + """ + Get closest index to lookahead that is greater than the lookahead + """ + global LOOKAHEAD + while find_distance_index_based(idx) < LOOKAHEAD: + idx += 1 + return idx - 1 + +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): + """ + Plot arrow + """ + if not isinstance(x, float): + for ix, iy, iyaw in zip(x, y, yaw): + plot_arrow(ix, iy, iyaw) + else: + plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc, ec=ec, head_width=width, head_length=width) + plt.plot(x, y) + patches.Rectangle((xc,yc), 0.35,0.2) + +def pure_pursuit(): + global xc, yc, yaw, vel, waypoints, v_prev_error, freqs, idx,LOOKAHEAD, WB, pure_pursuit_flag, show_animation + + # Initialize the message, subscriber and publisher + msg = Twist() + rospy.Subscriber("/odom", Odometry, pose_callback) + pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + cx = waypoints[:, 0]; cy = waypoints[:, 1] + + try: + while pure_pursuit_flag: + nearest_idx = find_nearest_waypoint() + idx_near_lookahead = idx_close_to_lookahead(nearest_idx) + target_x = float(waypoints[idx_near_lookahead][0]) + target_y = float(waypoints[idx_near_lookahead][1]) + + # Velocity PID controller + kp = 1.0 # 1.0 + kd = 0 + ki = 0 + + dt = 1.0 / freqs + v_desired = float(waypoints[nearest_idx][3]) + v_error = v_desired - vel + + P_vel = kp * v_error + I_vel = v_error * dt + D_vel = kd * (v_error - v_prev_error) / dt + + velocity = P_vel + I_vel + D_vel + v_prev_error = v_error + print(f"NEAREST INDEX = {nearest_idx}, output = {velocity}, velocity desired = {v_desired}, current = {vel}") + + + """ + PURE PURSUIT CONTROLLER + """ + + # calculate alpha (angle between the goal point and the path point) + x_delta = target_x - xc + y_delta = target_y - yc + alpha = np.arctan(y_delta / x_delta) - yaw + + # front of the vehicle is 0 degrees right +90 and left -90 hence we need to convert our alpha + if alpha > np.pi / 2: + alpha -= np.pi + if alpha < -np.pi / 2: + alpha += np.pi + + # Set the lookahead distance depending on the speed + lookahead = find_distance(target_x, target_y) + steering_angle = np.arctan((2 * WB * np.sin(alpha)) / lookahead) + + # Set max wheel turning angle + if steering_angle > 0.5: + steering_angle = 0.5 + elif steering_angle < -0.5: + steering_angle = -0.5 + + # Publish messages + msg.linear.x = velocity + msg.angular.z = steering_angle + pub.publish(msg) + + # Plot map progression + if show_animation: + plt.cla() + # For stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) + plot_arrow(float(xc), float(yc), float(yaw)) + plt.plot(cx, cy, "-r", label = "course") + plt.plot(xc, yc, "-b", label = "trajectory") + plt.plot(target_x, target_y, "xg", label = "target") + plt.axis("equal") + plt.grid(True) + plt.title("Pure Pursuit Control" + str(1)) + plt.pause(0.001) + + except IndexError: + print("PURE PURSUIT COMPLETE --> COMPLETED ALL WAYPOINTS") + + +if __name__=='__main__': + rospy.init_node('pure_pursuit') + r = rospy.Rate(freqs) + print("RUNNING PURE-PURSUIT CODE.... \n\n") + time.sleep(2) + waypoints = read_points() + pure_pursuit() \ No newline at end of file From c0af0cca85f1bfb72860a61a5bd452bda26d772a Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Wed, 3 Apr 2024 14:33:18 -0500 Subject: [PATCH 02/22] Setup 2 --- src/control/pure_pursuit/pure_pursuit_node.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/control/pure_pursuit/pure_pursuit_node.py b/src/control/pure_pursuit/pure_pursuit_node.py index dac303b1..af2105ce 100644 --- a/src/control/pure_pursuit/pure_pursuit_node.py +++ b/src/control/pure_pursuit/pure_pursuit_node.py @@ -16,7 +16,6 @@ from tf.transformations import euler_from_quaternion, quaternion_from_euler from geometry_msgs.msg import PoseStamped, Point, Twist, Quaternion, Pose, Point, Vector3 - # GLOBAL VARIABLES xc = 0.0 yc = 0.0 @@ -37,6 +36,9 @@ pure_pursuit_flag = True show_animation = True +# TODO +# Set path subscriber here +# /planning/trajectory def read_points(): """ CHANGE THIS PATH TO WHERE YOU HAVE SAVED YOUR CSV FILES @@ -115,8 +117,12 @@ def pure_pursuit(): # Initialize the message, subscriber and publisher msg = Twist() + + # TODO + # Change to navigator messages + # Message to subscribe I think -> /radar_1/can_viz_markers rospy.Subscriber("/odom", Odometry, pose_callback) - pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + pub = rospy.Publisher('pure_pursuit/cmd_vel', Twist, queue_size=1) cx = waypoints[:, 0]; cy = waypoints[:, 1] @@ -145,10 +151,8 @@ def pure_pursuit(): print(f"NEAREST INDEX = {nearest_idx}, output = {velocity}, velocity desired = {v_desired}, current = {vel}") - """ - PURE PURSUIT CONTROLLER - """ - + # PURE PURSUIT CONTROLLER + # calculate alpha (angle between the goal point and the path point) x_delta = target_x - xc y_delta = target_y - yc @@ -190,7 +194,7 @@ def pure_pursuit(): plt.pause(0.001) except IndexError: - print("PURE PURSUIT COMPLETE --> COMPLETED ALL WAYPOINTS") + print("PURE PURSUIT COMPLETE --> COMPLETED ALL WAYPOINTS") if __name__=='__main__': From c125dd02d2d7ca44c6c4d754f2223ba030c8575d Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Sun, 7 Apr 2024 11:10:19 -0500 Subject: [PATCH 03/22] Setting up throttle and brake logic --- src/control/pure_pursuit/pure_pursuit_node.py | 43 ++++++++++++++++--- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/src/control/pure_pursuit/pure_pursuit_node.py b/src/control/pure_pursuit/pure_pursuit_node.py index af2105ce..08152351 100644 --- a/src/control/pure_pursuit/pure_pursuit_node.py +++ b/src/control/pure_pursuit/pure_pursuit_node.py @@ -36,9 +36,30 @@ pure_pursuit_flag = True show_animation = True +# PID Logic inferred from rtp_node.py +# TODO +# Update to newer PID logic +def calculate_throttle(current_speed, target_speed): + #target_speed = 1.5 # MAX_SPEED # m/s, ~10mph + if(target_speed > 1.5): + target_speed = 1.5 + + pid_error = target_speed - current_speed + # self.get_logger().info(f"Steer/Pose: {command.steer}/{((best_path.poses[4][2] + best_path.poses[5][2] + best_path.poses[6][2])/3.0)}") + # print(f"Speed: {self.speed} / {target_speed}") + if pid_error > 0: + command.throttle = min(pid_error *0.5, 0.6) + command.brake = 0.0 + #else: + elif pid_error <= -1: + brake = pid_error *0.6 *-1.0 + throttle = 0.0 + + return throttle, brake + # TODO # Set path subscriber here -# /planning/trajectory +# /planning/path def read_points(): """ CHANGE THIS PATH TO WHERE YOU HAVE SAVED YOUR CSV FILES @@ -121,8 +142,8 @@ def pure_pursuit(): # TODO # Change to navigator messages # Message to subscribe I think -> /radar_1/can_viz_markers - rospy.Subscriber("/odom", Odometry, pose_callback) - pub = rospy.Publisher('pure_pursuit/cmd_vel', Twist, queue_size=1) + rospy.Subscriber("/gnss/odometry", Odometry, pose_callback) + pub = rospy.Publisher('/vehicle/control', Twist, queue_size=1) cx = waypoints[:, 0]; cy = waypoints[:, 1] @@ -150,7 +171,9 @@ def pure_pursuit(): v_prev_error = v_error print(f"NEAREST INDEX = {nearest_idx}, output = {velocity}, velocity desired = {v_desired}, current = {vel}") - + # Setting variables to publish + throttle, brake = calculate_throttle(vel, v_desired) + # PURE PURSUIT CONTROLLER # calculate alpha (angle between the goal point and the path point) @@ -175,8 +198,9 @@ def pure_pursuit(): steering_angle = -0.5 # Publish messages - msg.linear.x = velocity - msg.angular.z = steering_angle + msg.Steer = steering_angle + msg.Throttle = throttle + msg.Brake = brake pub.publish(msg) # Plot map progression @@ -197,10 +221,15 @@ def pure_pursuit(): print("PURE PURSUIT COMPLETE --> COMPLETED ALL WAYPOINTS") + if __name__=='__main__': rospy.init_node('pure_pursuit') r = rospy.Rate(freqs) print("RUNNING PURE-PURSUIT CODE.... \n\n") time.sleep(2) waypoints = read_points() - pure_pursuit() \ No newline at end of file + pure_pursuit() + +# path message (points x y) -> Steering angle, desired velocity +# pid -> desired + From 46ce9cddfb0a964523f0ed06fd659826b367a134 Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Sun, 7 Apr 2024 14:40:00 -0500 Subject: [PATCH 04/22] Added path subscriber --- src/control/pure_pursuit/pure_pursuit_node.py | 28 ++++++++++--------- src/planning/rtp/rtp/rtp_node.py | 2 +- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/control/pure_pursuit/pure_pursuit_node.py b/src/control/pure_pursuit/pure_pursuit_node.py index 08152351..05a40111 100644 --- a/src/control/pure_pursuit/pure_pursuit_node.py +++ b/src/control/pure_pursuit/pure_pursuit_node.py @@ -38,9 +38,12 @@ # PID Logic inferred from rtp_node.py # TODO -# Update to newer PID logic +# Update to newer PID logic and check for target_speed unit +# Fine tuning def calculate_throttle(current_speed, target_speed): #target_speed = 1.5 # MAX_SPEED # m/s, ~10mph + + # Max speed check if(target_speed > 1.5): target_speed = 1.5 @@ -60,17 +63,19 @@ def calculate_throttle(current_speed, target_speed): # TODO # Set path subscriber here # /planning/path -def read_points(): +def path_callback(data): """ CHANGE THIS PATH TO WHERE YOU HAVE SAVED YOUR CSV FILES """ - r = rospkg.RosPack() - package_path = r.get_path('pure_pursuit') - file_name = 'wp_file.csv' #'racecar_walker.csv' - file_path = package_path + '/waypoints/' + file_name - with open(file_path) as f: - path_points = np.loadtxt(file_path, delimiter = ',') - return path_points + # r = rospkg.RosPack() + # package_path = r.get_path('pure_pursuit') + # file_name = 'wp_file.csv' #'racecar_walker.csv' + # file_path = package_path + '/waypoints/' + file_name + # with open(file_path) as f: + # path_points = np.loadtxt(file_path, delimiter = ',') + # return path_points + + waypoints = data.poses def pose_callback(data): """ @@ -139,10 +144,8 @@ def pure_pursuit(): # Initialize the message, subscriber and publisher msg = Twist() - # TODO - # Change to navigator messages - # Message to subscribe I think -> /radar_1/can_viz_markers rospy.Subscriber("/gnss/odometry", Odometry, pose_callback) + rospy.Subscriber("/planning/path", Path, path_callback) pub = rospy.Publisher('/vehicle/control', Twist, queue_size=1) cx = waypoints[:, 0]; cy = waypoints[:, 1] @@ -227,7 +230,6 @@ def pure_pursuit(): r = rospy.Rate(freqs) print("RUNNING PURE-PURSUIT CODE.... \n\n") time.sleep(2) - waypoints = read_points() pure_pursuit() # path message (points x y) -> Steering angle, desired velocity diff --git a/src/planning/rtp/rtp/rtp_node.py b/src/planning/rtp/rtp/rtp_node.py index 0a22af42..64d6778a 100755 --- a/src/planning/rtp/rtp/rtp_node.py +++ b/src/planning/rtp/rtp/rtp_node.py @@ -536,7 +536,7 @@ def costMapCb(self, msg: OccupancyGrid): if np.sqrt(pose[0]**2 + pose[1]**2) > lookahead_distance: lookahead_pose = pose break - + if lookahead_pose is None: lookahead_pose = poses_np_bl[-1] From d39132e8c4d86a1ad93157021a90d1697ac84a6e Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Sun, 7 Apr 2024 15:11:27 -0500 Subject: [PATCH 05/22] feat: purse pursuit controller w/ waypoint finding --- launches/launch.vehicle.py | 1 + launches/launch_node_definitions.py | 5 + .../pure_pursuit_controller/package.xml | 18 +++ .../pure_pursuit_controller/__init__.py | 0 .../pure_pursuit_controller_node.py | 130 ++++++++++++++++++ .../resource/pure_pursuit_controller | 0 src/control/pure_pursuit_controller/setup.cfg | 4 + src/control/pure_pursuit_controller/setup.py | 25 ++++ .../test/test_copyright.py | 25 ++++ .../test/test_flake8.py | 25 ++++ .../test/test_pep257.py | 23 ++++ 11 files changed, 256 insertions(+) create mode 100755 src/control/pure_pursuit_controller/package.xml create mode 100755 src/control/pure_pursuit_controller/pure_pursuit_controller/__init__.py create mode 100755 src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py create mode 100755 src/control/pure_pursuit_controller/resource/pure_pursuit_controller create mode 100755 src/control/pure_pursuit_controller/setup.cfg create mode 100755 src/control/pure_pursuit_controller/setup.py create mode 100755 src/control/pure_pursuit_controller/test/test_copyright.py create mode 100755 src/control/pure_pursuit_controller/test/test_flake8.py create mode 100755 src/control/pure_pursuit_controller/test/test_pep257.py diff --git a/launches/launch.vehicle.py b/launches/launch.vehicle.py index 736b2449..b2b55a89 100644 --- a/launches/launch.vehicle.py +++ b/launches/launch.vehicle.py @@ -57,6 +57,7 @@ def generate_launch_description(): grid_summation, junction_manager, rtp, + pure_pursuit_controller, # SAFETY ##airbags, diff --git a/launches/launch_node_definitions.py b/launches/launch_node_definitions.py index bb33ca26..28f946bf 100644 --- a/launches/launch_node_definitions.py +++ b/launches/launch_node_definitions.py @@ -4,6 +4,11 @@ NAVIGATOR_DIR = "/navigator/" +pure_pursuit_controller = Node( + package='pure_pursuit_controller', + executable='pure_pursuit_controller' +) + airbags = Node( package='airbags', executable='airbag_node' diff --git a/src/control/pure_pursuit_controller/package.xml b/src/control/pure_pursuit_controller/package.xml new file mode 100755 index 00000000..06fbf7d9 --- /dev/null +++ b/src/control/pure_pursuit_controller/package.xml @@ -0,0 +1,18 @@ + + + + pure_pursuit_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/__init__.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py new file mode 100755 index 00000000..552b82ee --- /dev/null +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -0,0 +1,130 @@ +import math + +import rclpy +from rclpy.node import Node + +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Pose, PoseStamped, Point + +from navigator_msgs.msg import VehicleControl, VehicleSpeed + + +class Constants: + LD: float = 6.0 # lookahead distance in meters + kf: float = 0.1 # look forward gain in meters + + +class VehicleState: + def __init__(self): + self.pose: Pose | None = None + self.velocity: float | None = None + + def loaded(self) -> bool: + return self.pose is not None and self.velocity is not None + + def lookahead_distance(self) -> float: + return Constants.kf * self.velocity + Constants.LD + + def _calc_distance(self, x: float, y: float) -> float: + return math.hypot( + x - self.pose.position.x, + y - self.pose.position.y, + ) + + +class PursuitPath: + def __init__(self, cx: list[float] = [], cy: list[float] = []): + self.cx = cx + self.cy = cy + self.prev_index: int | None = None + + def _calc_nearest_index(self, vs: VehicleState) -> int: + if len(self.cx) == 0 or len(self.cy) == 0 or not vs.loaded(): + return + + min_dist = float("inf") + min_index = 0 + for i in range(len(self.cx)): + dist = vs._calc_distance(self.cx[i], self.cy[i]) + if dist < min_dist: + min_dist = dist + min_index = i + + return min_index + + def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: + if len(self.cx) == 0 or len(self.cy) == 0 or not vs.loaded(): + return + + if self.prev_index is None: + self.prev_index = self._calc_nearest_index(vs) + + for i in range(self.prev_index, len(self.cx)): + dist = vs._calc_distance(self.cx[i], self.cy[i]) + if dist > vs.lookahead_distance(): + self.prev_index = i + return [self.cx[i], self.cy[i]] + +class PursePursuitController(Node): + + def __init__(self): + super().__init__("pure_pursuit_controler") + + self.vehicle_state = VehicleState() + self.path = PursuitPath() + self.target_waypoint = None + + self.first_route = None + + self.route_subscriber = self.create_subscription( + Path, "/planning/route", self.route_callback, 1 + ) + self.odometry_subscriber = self.create_subscription( + Odometry, "/gnss/odometry", self.odometry_callback, 1 + ) + self.speed_subscriber = self.create_subscription( + VehicleSpeed, "/speed", self.speed_callback, 1 + ) + + self.timer = self.create_timer(0.5, self.timer_callback) + + def route_callback(self, msg: Path): + # Only use the first route for now. TODO: how will routes be updated in the future? + if self.first_route is None: + self.first_route = msg + route = [ + [pose_stamped.pose.position.x, pose_stamped.pose.position.y] + for pose_stamped in msg.poses + ] + self.get_logger().info(f"Route: {route}\n") + self.path.cx = [pose_stamped.pose.position.x for pose_stamped in msg.poses] + self.path.cy = [pose_stamped.pose.position.y for pose_stamped in msg.poses] + + def odometry_callback(self, msg: Odometry): + self.vehicle_state.pose = msg.pose.pose + + def speed_callback(self, msg: VehicleSpeed): + self.vehicle_state.velocity = msg.speed + + def timer_callback(self): + self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + if self.target_waypoint is not None: + self.get_logger().info(f"Target waypoint: {self.target_waypoint}") + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = PursePursuitController() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/control/pure_pursuit_controller/resource/pure_pursuit_controller b/src/control/pure_pursuit_controller/resource/pure_pursuit_controller new file mode 100755 index 00000000..e69de29b diff --git a/src/control/pure_pursuit_controller/setup.cfg b/src/control/pure_pursuit_controller/setup.cfg new file mode 100755 index 00000000..f56125a2 --- /dev/null +++ b/src/control/pure_pursuit_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pure_pursuit_controller +[install] +install_scripts=$base/lib/pure_pursuit_controller diff --git a/src/control/pure_pursuit_controller/setup.py b/src/control/pure_pursuit_controller/setup.py new file mode 100755 index 00000000..78e03d9e --- /dev/null +++ b/src/control/pure_pursuit_controller/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "pure_pursuit_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="root", + maintainer_email="root@todo.todo", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "pure_pursuit_controller = pure_pursuit_controller.pure_pursuit_controller_node:main" + ], + }, +) diff --git a/src/control/pure_pursuit_controller/test/test_copyright.py b/src/control/pure_pursuit_controller/test/test_copyright.py new file mode 100755 index 00000000..97a39196 --- /dev/null +++ b/src/control/pure_pursuit_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/pure_pursuit_controller/test/test_flake8.py b/src/control/pure_pursuit_controller/test/test_flake8.py new file mode 100755 index 00000000..27ee1078 --- /dev/null +++ b/src/control/pure_pursuit_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/pure_pursuit_controller/test/test_pep257.py b/src/control/pure_pursuit_controller/test/test_pep257.py new file mode 100755 index 00000000..b234a384 --- /dev/null +++ b/src/control/pure_pursuit_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' From aa4656b8d1b9e098417e126e0be46bb216373797 Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Sun, 7 Apr 2024 15:59:08 -0500 Subject: [PATCH 06/22] Added calculate_throttle_brake() --- .../pure_pursuit_controller_node.py | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 552b82ee..431370c8 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -13,6 +13,26 @@ class Constants: LD: float = 6.0 # lookahead distance in meters kf: float = 0.1 # look forward gain in meters +def calculate_throttle_brake(current_speed, target_speed): + + delta = 0.6 # Controls how steep throttle and acceleration will be + maxSpeed = 1.5 # MAX SPEED ~ 10mph + + # Max speed is set to 1.5 + if(target_speed > maxSpeed): + target_speed = maxSpeed + + # PID logic sources from RTP node + pid_error = target_speed - current_speed + + if pid_error > 0: + throttle = min(pid_error * 0.5, delta) + brake = 0.0 + elif pid_error <= 0: + brake = pid_error * delta * -1.0 + throttle = 0.0 + + return throttle, brake class VehicleState: def __init__(self): From a8b41327564a76fb2552cbf5eecbbf369926cf3d Mon Sep 17 00:00:00 2001 From: Ayush-sharma-py Date: Sun, 7 Apr 2024 16:01:36 -0500 Subject: [PATCH 07/22] Typo --- .../pure_pursuit_controller/pure_pursuit_controller_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 431370c8..b284417c 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -15,7 +15,7 @@ class Constants: def calculate_throttle_brake(current_speed, target_speed): - delta = 0.6 # Controls how steep throttle and acceleration will be + delta = 0.6 # Controls max throttle and acceleration will be maxSpeed = 1.5 # MAX SPEED ~ 10mph # Max speed is set to 1.5 From 955c0cb240732b4ccd3753db863472ab46faa114 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Mon, 8 Apr 2024 17:15:36 -0500 Subject: [PATCH 08/22] feat: integrate RTP throttle/break into pure pursuit controller --- .../pure_pursuit_controller_node.py | 74 +++++++++++++------ 1 file changed, 51 insertions(+), 23 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 431370c8..7f9ac189 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -12,27 +12,9 @@ class Constants: LD: float = 6.0 # lookahead distance in meters kf: float = 0.1 # look forward gain in meters + THROTTLE_DELTA = 0.6 # Controls how steep throttle and acceleration will be + MAX_SPEED: float = 1.5 # Max speed ~ 10mph -def calculate_throttle_brake(current_speed, target_speed): - - delta = 0.6 # Controls how steep throttle and acceleration will be - maxSpeed = 1.5 # MAX SPEED ~ 10mph - - # Max speed is set to 1.5 - if(target_speed > maxSpeed): - target_speed = maxSpeed - - # PID logic sources from RTP node - pid_error = target_speed - current_speed - - if pid_error > 0: - throttle = min(pid_error * 0.5, delta) - brake = 0.0 - elif pid_error <= 0: - brake = pid_error * delta * -1.0 - throttle = 0.0 - - return throttle, brake class VehicleState: def __init__(self): @@ -42,6 +24,34 @@ def __init__(self): def loaded(self) -> bool: return self.pose is not None and self.velocity is not None + def calc_throttle_brake(self) -> tuple[float, float] | None: + if not self.loaded(): + return + + # Our target speed is the max speed + pid_error = Constants.MAX_SPEED - self.velocity + if pid_error > 0: + throttle = min(pid_error * 0.5, Constants.THROTTLE_DELTA) + brake = 0.0 + elif pid_error <= 0: + brake = pid_error * Constants.THROTTLE_DELTA * -1.0 + throttle = 0.0 + + return throttle, brake + + def calc_steer(self, target: tuple[float, float]) -> float: + if not self.loaded(): + return + + dx = target[0] - self.pose.position.x + dy = target[1] - self.pose.position.y + + alpha = math.atan2(dy, dx) - self.pose.orientation.z + delta = math.atan2( + 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() + ) + return delta + def lookahead_distance(self) -> float: return Constants.kf * self.velocity + Constants.LD @@ -70,7 +80,7 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: min_dist = dist min_index = i - return min_index + return min_index def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: if len(self.cx) == 0 or len(self.cy) == 0 or not vs.loaded(): @@ -85,6 +95,7 @@ def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: self.prev_index = i return [self.cx[i], self.cy[i]] + class PursePursuitController(Node): def __init__(self): @@ -106,7 +117,12 @@ def __init__(self): VehicleSpeed, "/speed", self.speed_callback, 1 ) - self.timer = self.create_timer(0.5, self.timer_callback) + self.command_publisher = self.create_publisher( + VehicleControl, "/control/vehicle_control", 1 + ) + + self.waypoint_timer = self.create_timer(0.5, self.waypoint_callback) + self.control_timer = self.create_timer(0.1, self.control_callback) def route_callback(self, msg: Path): # Only use the first route for now. TODO: how will routes be updated in the future? @@ -126,11 +142,23 @@ def odometry_callback(self, msg: Odometry): def speed_callback(self, msg: VehicleSpeed): self.vehicle_state.velocity = msg.speed - def timer_callback(self): + def waypoint_callback(self): self.target_waypoint = self.path.calc_target_point(self.vehicle_state) if self.target_waypoint is not None: self.get_logger().info(f"Target waypoint: {self.target_waypoint}") + def control_callback(self): + throttle_brake = self.vehicle_state.calc_throttle_brake() + if throttle_brake is not None: + throttle, brake = throttle_brake + # steer = self.vehicle_state.calc_steer(self.target_waypoint) + control_msg = VehicleControl() + control_msg.throttle = throttle + control_msg.brake = brake + # control_msg.steer = steer + self.get_logger().info(f"Control message: {control_msg}") + self.command_publisher.publish(control_msg) + def main(args=None): rclpy.init(args=args) From 6ae07fc32a2c4bc0b365f20a4a4faeee422f1623 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Mon, 8 Apr 2024 19:03:22 -0500 Subject: [PATCH 09/22] fix: vehicle control pub path --- .../pure_pursuit_controller/pure_pursuit_controller_node.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index b85d45ec..3053cfae 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -118,7 +118,7 @@ def __init__(self): ) self.command_publisher = self.create_publisher( - VehicleControl, "/control/vehicle_control", 1 + VehicleControl, "/vehicle/control", 1 ) self.waypoint_timer = self.create_timer(0.5, self.waypoint_callback) @@ -156,7 +156,9 @@ def control_callback(self): control_msg.throttle = throttle control_msg.brake = brake # control_msg.steer = steer - self.get_logger().info(f"Control message: {control_msg}") + self.get_logger().info( + f"Steer: {control_msg.steer} Throttle: {throttle} Brake: {brake}" + ) self.command_publisher.publish(control_msg) From a9a29a96306642dc1cfd36d3eda55d396f6aac6c Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Mon, 8 Apr 2024 20:20:33 -0500 Subject: [PATCH 10/22] feat: partially working steering --- .../pure_pursuit_controller_node.py | 64 ++++++++++++------- 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 3053cfae..a9d2109e 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -10,14 +10,21 @@ class Constants: - LD: float = 6.0 # lookahead distance in meters - kf: float = 0.1 # look forward gain in meters - MAX_THROTTLE = 0.6 # Controls max throttle and acceleration will be - MAX_SPEED: float = 1.5 # Max speed ~ 10mph + # Look ahead distance in meters + LD: float = 6.0 + # Look forward gain in meters (gain in look ahead distance per m/s of speed) + kf: float = 0.1 + # Wheel base (distance between front and rear wheels) in meter + WHEEL_BASE: float = 3.5 + # Max throttle and acceleration + MAX_THROTTLE = 0.6 + # Max speed in m/s + MAX_SPEED: float = 1.5 class VehicleState: - def __init__(self): + def __init__(self, l): + self.l = l self.pose: Pose | None = None self.velocity: float | None = None @@ -39,18 +46,23 @@ def calc_throttle_brake(self) -> tuple[float, float] | None: return throttle, brake - def calc_steer(self, target: tuple[float, float]) -> float: + def calc_steer(self, target: tuple[float, float]) -> float | None: if not self.loaded(): return dx = target[0] - self.pose.position.x dy = target[1] - self.pose.position.y - alpha = math.atan2(dy, dx) - self.pose.orientation.z + alpha = math.atan2(dy, dx) + self.l.info( + f"Target: {target}. (dx, dy): ({dx}, {dy}). Alpha: {alpha}. ZOrientation: {self.pose.orientation.z}" + ) delta = math.atan2( 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() ) - return delta + + # Normalize the steering angle from [-pi, pi] to [-1, 1] + return -delta / math.pi def lookahead_distance(self) -> float: return Constants.kf * self.velocity + Constants.LD @@ -101,7 +113,7 @@ class PursePursuitController(Node): def __init__(self): super().__init__("pure_pursuit_controler") - self.vehicle_state = VehicleState() + self.vehicle_state = VehicleState(l=self.get_logger()) self.path = PursuitPath() self.target_waypoint = None @@ -144,22 +156,30 @@ def speed_callback(self, msg: VehicleSpeed): def waypoint_callback(self): self.target_waypoint = self.path.calc_target_point(self.vehicle_state) - if self.target_waypoint is not None: - self.get_logger().info(f"Target waypoint: {self.target_waypoint}") def control_callback(self): + # If no target waypoint, do nothing. + if not self.target_waypoint: + return + + # Calculate throttle, brake, and steer throttle_brake = self.vehicle_state.calc_throttle_brake() - if throttle_brake is not None: - throttle, brake = throttle_brake - # steer = self.vehicle_state.calc_steer(self.target_waypoint) - control_msg = VehicleControl() - control_msg.throttle = throttle - control_msg.brake = brake - # control_msg.steer = steer - self.get_logger().info( - f"Steer: {control_msg.steer} Throttle: {throttle} Brake: {brake}" - ) - self.command_publisher.publish(control_msg) + steer = self.vehicle_state.calc_steer(self.target_waypoint) + + # If vehicle state is not loaded, do nothing. + if not throttle_brake or not steer: + return + + # Set throttle, brake, and steer and publish. + throttle, brake = throttle_brake + control_msg = VehicleControl() + control_msg.throttle = throttle + control_msg.brake = brake + control_msg.steer = steer + self.get_logger().info( + f"Steer: {control_msg.steer} Throttle: {throttle} Brake: {brake}" + ) + self.command_publisher.publish(control_msg) def main(args=None): From b18dc67ebeb0cf48086723b310601042ba40bb56 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Tue, 16 Apr 2024 17:03:18 -0500 Subject: [PATCH 11/22] fix: steer should account for yaw --- .../pure_pursuit_controller_node.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index a9d2109e..5c9e92e8 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -1,7 +1,9 @@ import math import rclpy + from rclpy.node import Node +from tf_transformer import euler_from_quaternion from nav_msgs.msg import Path, Odometry from geometry_msgs.msg import Pose, PoseStamped, Point @@ -53,10 +55,16 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: dx = target[0] - self.pose.position.x dy = target[1] - self.pose.position.y - alpha = math.atan2(dy, dx) - self.l.info( - f"Target: {target}. (dx, dy): ({dx}, {dy}). Alpha: {alpha}. ZOrientation: {self.pose.orientation.z}" + _, _, yaw = euler_from_quaternion( + [ + self.pose.orientation.x, + self.pose.orientation.y, + self.pose.orientation.z, + self.pose.orientation.w, + ] ) + + alpha = math.atan2(dy, dx) - yaw delta = math.atan2( 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() ) From 847f31536b1172eaf2b53b515cbed233710475b1 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Fri, 19 Apr 2024 13:41:58 -0500 Subject: [PATCH 12/22] fix: pure pursuit steering normalization by pi/2 instead of pi --- .../pure_pursuit_controller/pure_pursuit_controller_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 5c9e92e8..7712cc63 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -3,7 +3,7 @@ import rclpy from rclpy.node import Node -from tf_transformer import euler_from_quaternion +from tf_transformations import euler_from_quaternion from nav_msgs.msg import Path, Odometry from geometry_msgs.msg import Pose, PoseStamped, Point @@ -13,7 +13,7 @@ class Constants: # Look ahead distance in meters - LD: float = 6.0 + LD: float = 3.0 # Look forward gain in meters (gain in look ahead distance per m/s of speed) kf: float = 0.1 # Wheel base (distance between front and rear wheels) in meter @@ -70,7 +70,7 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: ) # Normalize the steering angle from [-pi, pi] to [-1, 1] - return -delta / math.pi + return -delta / (math.pi / 2) def lookahead_distance(self) -> float: return Constants.kf * self.velocity + Constants.LD From caefa171f8565e7394de379685241c8fe7d4dba8 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Fri, 19 Apr 2024 15:59:03 -0500 Subject: [PATCH 13/22] refactor: pure pursuit merge cx and cy into single path list --- .../pure_pursuit_controller_node.py | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 7712cc63..70fd0d08 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -83,19 +83,18 @@ def _calc_distance(self, x: float, y: float) -> float: class PursuitPath: - def __init__(self, cx: list[float] = [], cy: list[float] = []): - self.cx = cx - self.cy = cy + def __init__(self, path: list[tuple[float, float]] = []): + self.path = path self.prev_index: int | None = None def _calc_nearest_index(self, vs: VehicleState) -> int: - if len(self.cx) == 0 or len(self.cy) == 0 or not vs.loaded(): + if len(self.path) == 0 or not vs.loaded(): return min_dist = float("inf") min_index = 0 - for i in range(len(self.cx)): - dist = vs._calc_distance(self.cx[i], self.cy[i]) + for i, (x, y) in enumerate(self.path): + dist = vs._calc_distance(x, y) if dist < min_dist: min_dist = dist min_index = i @@ -103,17 +102,17 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: return min_index def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: - if len(self.cx) == 0 or len(self.cy) == 0 or not vs.loaded(): + if len(self.path) == 0 or not vs.loaded(): return if self.prev_index is None: self.prev_index = self._calc_nearest_index(vs) - for i in range(self.prev_index, len(self.cx)): - dist = vs._calc_distance(self.cx[i], self.cy[i]) + for i, (x, y) in enumerate(self.path[self.prev_index :]): + dist = vs._calc_distance(x, y) if dist > vs.lookahead_distance(): - self.prev_index = i - return [self.cx[i], self.cy[i]] + self.prev_index += i + return (x, y) class PursePursuitController(Node): @@ -149,12 +148,11 @@ def route_callback(self, msg: Path): if self.first_route is None: self.first_route = msg route = [ - [pose_stamped.pose.position.x, pose_stamped.pose.position.y] + (pose_stamped.pose.position.x, pose_stamped.pose.position.y) for pose_stamped in msg.poses ] self.get_logger().info(f"Route: {route}\n") - self.path.cx = [pose_stamped.pose.position.x for pose_stamped in msg.poses] - self.path.cy = [pose_stamped.pose.position.y for pose_stamped in msg.poses] + self.path.path = route def odometry_callback(self, msg: Odometry): self.vehicle_state.pose = msg.pose.pose From bb89258dd94e2b2b41b5e7d2bc63aaa4979fb959 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Tue, 23 Apr 2024 17:56:42 -0500 Subject: [PATCH 14/22] feat: add rviz visualizations [partially working] --- data/navigator_default.rviz | 27 +++- .../pure_pursuit_controller_node.py | 124 +++++++++++++++++- 2 files changed, 138 insertions(+), 13 deletions(-) diff --git a/data/navigator_default.rviz b/data/navigator_default.rviz index 6f017558..811b6f2c 100644 --- a/data/navigator_default.rviz +++ b/data/navigator_default.rviz @@ -8,8 +8,10 @@ Panels: - /Routes Paths1 - /Cameras1 - /Grids1 + - /Marker1 + - /Marker1/Status1 Splitter Ratio: 0.5 - Tree Height: 960 + Tree Height: 603 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -237,9 +239,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.9825243353843689 + Max Intensity: 0.9904967546463013 Min Color: 0; 73; 112 - Min Intensity: 0.7125484347343445 + Min Intensity: 0.7124737501144409 Name: Filtered LiDAR Position Transformer: XYZ Selectable: true @@ -537,6 +539,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /radar/radar_viz Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + lookahead: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/barrier_marker + Value: true Enabled: true Global Options: Background Color: 222; 222; 222 @@ -616,16 +631,16 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1373 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 2176 + Width: 1846 X: 74 Y: 27 diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 70fd0d08..e561031a 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -1,12 +1,18 @@ import math import rclpy +import numpy as np + +from numpy import typing as npt from rclpy.node import Node from tf_transformations import euler_from_quaternion +from std_msgs.msg import ColorRGBA +from rosgraph_msgs.msg import Clock from nav_msgs.msg import Path, Odometry from geometry_msgs.msg import Pose, PoseStamped, Point +from visualization_msgs.msg import Marker from navigator_msgs.msg import VehicleControl, VehicleSpeed @@ -84,8 +90,8 @@ def _calc_distance(self, x: float, y: float) -> float: class PursuitPath: def __init__(self, path: list[tuple[float, float]] = []): - self.path = path - self.prev_index: int | None = None + self.path: npt.NDArray[np.float64] = np.asarray(path) + self.current_path_range: tuple[int, int] = (0, 0) def _calc_nearest_index(self, vs: VehicleState) -> int: if len(self.path) == 0 or not vs.loaded(): @@ -101,17 +107,23 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: return min_index + def get_current_path(self) -> npt.NDArray[np.float64]: + return np.copy( + self.path[self.current_path_range[0] : self.current_path_range[1] + 1] + ) + def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: if len(self.path) == 0 or not vs.loaded(): return - if self.prev_index is None: - self.prev_index = self._calc_nearest_index(vs) + if self.current_path_range == (0, 0): + self.current_path_range = (0, self._calc_nearest_index(vs)) - for i, (x, y) in enumerate(self.path[self.prev_index :]): + current_path_end = self.current_path_range[1] + for i, (x, y) in enumerate(self.path[current_path_end:]): dist = vs._calc_distance(x, y) if dist > vs.lookahead_distance(): - self.prev_index += i + self.current_path_range = (current_path_end, current_path_end + i) return (x, y) @@ -135,14 +147,24 @@ def __init__(self): self.speed_subscriber = self.create_subscription( VehicleSpeed, "/speed", self.speed_callback, 1 ) + self.clock_subscriber = self.create_subscription( + Clock, "/clock", self.clock_callback, 1 + ) self.command_publisher = self.create_publisher( VehicleControl, "/vehicle/control", 1 ) + self.rviz_path_publisher = self.create_publisher(Path, "/planning/path", 1) + self.barrier_marker_pub = self.create_publisher( + Marker, "/planning/barrier_marker", 1 + ) self.waypoint_timer = self.create_timer(0.5, self.waypoint_callback) self.control_timer = self.create_timer(0.1, self.control_callback) + def clock_callback(self, msg: Clock): + self.clock = msg.clock + def route_callback(self, msg: Path): # Only use the first route for now. TODO: how will routes be updated in the future? if self.first_route is None: @@ -152,7 +174,7 @@ def route_callback(self, msg: Path): for pose_stamped in msg.poses ] self.get_logger().info(f"Route: {route}\n") - self.path.path = route + self.path.path = np.asarray(route) def odometry_callback(self, msg: Odometry): self.vehicle_state.pose = msg.pose.pose @@ -162,6 +184,94 @@ def speed_callback(self, msg: VehicleSpeed): def waypoint_callback(self): self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + current_path = self.path.get_current_path() + + if len(current_path) == 0: + return + + vehicle_origin = np.asarray( + [self.vehicle_state.pose.position.x + 5, self.vehicle_state.pose.position.y] + ) + + # Each RVIZ grid cell is 0.4m. + relative_waypoint = 0.4 * (np.asarray(self.target_waypoint) - vehicle_origin) + + self.publishLookaheadMarker( + self.vehicle_state.lookahead_distance(), relative_waypoint + ) + + path_msg = Path() + path_msg.header.frame_id = "base_link" + path_msg.header.stamp = self.clock + + vehicle_origin = np.asarray( + [self.vehicle_state.pose.position.x + 5, self.vehicle_state.pose.position.y] + ) + + current_path[:, 0] -= vehicle_origin[0] + current_path[:, 1] -= vehicle_origin[1] + current_path *= 0.4 # Each RVIZ grid cell is 0.4m. + + path_msg.poses = [ + PoseStamped( + header=path_msg.header, + pose=Pose(position=Point(x=x - 20, y=y - 30)), + ) + for x, y in current_path + ] + + self.get_logger().info( + f"Current Path: {current_path}. Target: {self.target_waypoint}" + ) + + self.rviz_path_publisher.publish(path_msg) + + def publishLookaheadMarker(self, radius: float, pose: tuple[float, float]): + marker = Marker() + marker.header.frame_id = "base_link" + marker.header.stamp = self.clock + marker.ns = "lookahead" + marker.id = 0 + marker.type = Marker.CYLINDER + + marker.action = Marker.ADD + + marker.scale.x = radius * 2 + marker.scale.y = radius * 2 + marker.scale.z = 0.2 + + color = ColorRGBA() + color.a = 0.3 + color.g = 1.0 + color.b = 1.0 + marker.color = color + + self.barrier_marker_pub.publish(marker) + + marker.id = 1 + marker.type = Marker.ARROW + + marker.action = Marker.ADD + + marker.scale.x = 0.5 + marker.scale.y = 0.8 + marker.scale.z = 0.3 + + color = ColorRGBA() + color.a = 0.7 + color.g = 1.0 + color.b = 0.6 + marker.color = color + + pt_a = Point() + marker.points.append(pt_a) + + pt_b = Point() + pt_b.x = pose[0] + pt_b.y = pose[1] + marker.points.append(pt_b) + + self.barrier_marker_pub.publish(marker) def control_callback(self): # If no target waypoint, do nothing. From fe934cb6f04bce5cdadaabfc47cd4ece9136808e Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Wed, 24 Apr 2024 16:26:26 -0500 Subject: [PATCH 15/22] fix: pure pursuit integration with nav2 path planning (don't account for yaw) --- .../pure_pursuit_controller_node.py | 116 ++++++++---------- 1 file changed, 51 insertions(+), 65 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index e561031a..56b02d3a 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -28,6 +28,8 @@ class Constants: MAX_THROTTLE = 0.6 # Max speed in m/s MAX_SPEED: float = 1.5 + # Path adjustment gain + PATH_ADJUSTMENT_GAIN: float = 0.5 class VehicleState: @@ -58,9 +60,6 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: if not self.loaded(): return - dx = target[0] - self.pose.position.x - dy = target[1] - self.pose.position.y - _, _, yaw = euler_from_quaternion( [ self.pose.orientation.x, @@ -70,7 +69,10 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: ] ) - alpha = math.atan2(dy, dx) - yaw + alpha = math.atan2(target[1], target[0]) + self.l.info( + f"Atan2: {math.atan2(target[1], target[0])} Yaw: {yaw} Alpha: {alpha}" + ) delta = math.atan2( 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() ) @@ -81,17 +83,16 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: def lookahead_distance(self) -> float: return Constants.kf * self.velocity + Constants.LD - def _calc_distance(self, x: float, y: float) -> float: - return math.hypot( - x - self.pose.position.x, - y - self.pose.position.y, - ) - class PursuitPath: - def __init__(self, path: list[tuple[float, float]] = []): + def __init__(self, l, path: list[tuple[float, float]] = []): + self.l = l self.path: npt.NDArray[np.float64] = np.asarray(path) - self.current_path_range: tuple[int, int] = (0, 0) + self.prev_index: int | None = None + + def set_path(self, path: list[tuple[float, float]]) -> None: + self.path = np.asarray(path) + self.prev_index = None def _calc_nearest_index(self, vs: VehicleState) -> int: if len(self.path) == 0 or not vs.loaded(): @@ -100,7 +101,7 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: min_dist = float("inf") min_index = 0 for i, (x, y) in enumerate(self.path): - dist = vs._calc_distance(x, y) + dist = math.hypot(x, y) if dist < min_dist: min_dist = dist min_index = i @@ -108,22 +109,19 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: return min_index def get_current_path(self) -> npt.NDArray[np.float64]: - return np.copy( - self.path[self.current_path_range[0] : self.current_path_range[1] + 1] - ) + return np.copy(self.path[: self.prev_index]) def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: if len(self.path) == 0 or not vs.loaded(): return - if self.current_path_range == (0, 0): - self.current_path_range = (0, self._calc_nearest_index(vs)) + if self.prev_index is None: + self.prev_index = self._calc_nearest_index(vs) - current_path_end = self.current_path_range[1] - for i, (x, y) in enumerate(self.path[current_path_end:]): - dist = vs._calc_distance(x, y) + for i, (x, y) in enumerate(self.path[self.prev_index :]): + dist = math.hypot(x, y) if dist > vs.lookahead_distance(): - self.current_path_range = (current_path_end, current_path_end + i) + self.prev_index = i return (x, y) @@ -133,13 +131,12 @@ def __init__(self): super().__init__("pure_pursuit_controler") self.vehicle_state = VehicleState(l=self.get_logger()) - self.path = PursuitPath() + self.path = PursuitPath(l=self.get_logger()) self.target_waypoint = None - - self.first_route = None + self.clock = Clock().clock self.route_subscriber = self.create_subscription( - Path, "/planning/route", self.route_callback, 1 + Path, "/planning/path", self.route_callback, 1 ) self.odometry_subscriber = self.create_subscription( Odometry, "/gnss/odometry", self.odometry_callback, 1 @@ -154,27 +151,28 @@ def __init__(self): self.command_publisher = self.create_publisher( VehicleControl, "/vehicle/control", 1 ) - self.rviz_path_publisher = self.create_publisher(Path, "/planning/path", 1) + self.barrier_marker_pub = self.create_publisher( Marker, "/planning/barrier_marker", 1 ) + self.lookahead_path_publisher = self.create_publisher(Path, "/ppc/path", 1) - self.waypoint_timer = self.create_timer(0.5, self.waypoint_callback) self.control_timer = self.create_timer(0.1, self.control_callback) + self.visualize_path_timer = self.create_timer(0.1, self.visualize_path_callback) + self.visualize_waypoint_timer = self.create_timer( + 0.1, self.visualize_waypoint_callback + ) def clock_callback(self, msg: Clock): self.clock = msg.clock def route_callback(self, msg: Path): - # Only use the first route for now. TODO: how will routes be updated in the future? - if self.first_route is None: - self.first_route = msg - route = [ - (pose_stamped.pose.position.x, pose_stamped.pose.position.y) - for pose_stamped in msg.poses - ] - self.get_logger().info(f"Route: {route}\n") - self.path.path = np.asarray(route) + path = [ + (pose_stamped.pose.position.x, pose_stamped.pose.position.y) + for pose_stamped in msg.poses + ] + self.path.set_path(path) + self.target_waypoint = self.path.calc_target_point(self.vehicle_state) def odometry_callback(self, msg: Odometry): self.vehicle_state.pose = msg.pose.pose @@ -182,49 +180,35 @@ def odometry_callback(self, msg: Odometry): def speed_callback(self, msg: VehicleSpeed): self.vehicle_state.velocity = msg.speed - def waypoint_callback(self): - self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + def visualize_waypoint_callback(self): + if self.target_waypoint is not None: + relative_waypoint = 0.4 * np.asarray(self.target_waypoint) + self.publishLookaheadMarker( + self.vehicle_state.lookahead_distance(), relative_waypoint + ) + + def visualize_path_callback(self): current_path = self.path.get_current_path() if len(current_path) == 0: return - vehicle_origin = np.asarray( - [self.vehicle_state.pose.position.x + 5, self.vehicle_state.pose.position.y] - ) - - # Each RVIZ grid cell is 0.4m. - relative_waypoint = 0.4 * (np.asarray(self.target_waypoint) - vehicle_origin) - - self.publishLookaheadMarker( - self.vehicle_state.lookahead_distance(), relative_waypoint - ) - path_msg = Path() path_msg.header.frame_id = "base_link" path_msg.header.stamp = self.clock - vehicle_origin = np.asarray( - [self.vehicle_state.pose.position.x + 5, self.vehicle_state.pose.position.y] - ) - - current_path[:, 0] -= vehicle_origin[0] - current_path[:, 1] -= vehicle_origin[1] - current_path *= 0.4 # Each RVIZ grid cell is 0.4m. + # Each RVIZ grid cell is 0.4m. + path = np.copy(np.asarray(current_path)) * 0.4 path_msg.poses = [ PoseStamped( header=path_msg.header, - pose=Pose(position=Point(x=x - 20, y=y - 30)), + pose=Pose(position=Point(x=x, y=y)), ) - for x, y in current_path + for x, y in path ] - self.get_logger().info( - f"Current Path: {current_path}. Target: {self.target_waypoint}" - ) - - self.rviz_path_publisher.publish(path_msg) + self.lookahead_path_publisher.publish(path_msg) def publishLookaheadMarker(self, radius: float, pose: tuple[float, float]): marker = Marker() @@ -282,8 +266,10 @@ def control_callback(self): throttle_brake = self.vehicle_state.calc_throttle_brake() steer = self.vehicle_state.calc_steer(self.target_waypoint) + self.get_logger().info(f"Steer: {steer} Throttle/Brake: {throttle_brake}") + # If vehicle state is not loaded, do nothing. - if not throttle_brake or not steer: + if throttle_brake is None or steer is None: return # Set throttle, brake, and steer and publish. From ce32d963cca2e6ea35986037b39f632fb1b466ce Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Wed, 24 Apr 2024 16:33:07 -0500 Subject: [PATCH 16/22] refactor: clean up rviz markers for ppc --- .../pure_pursuit_controller_node.py | 135 ++++++++---------- 1 file changed, 56 insertions(+), 79 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 56b02d3a..6a83b5df 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -8,10 +8,10 @@ from rclpy.node import Node from tf_transformations import euler_from_quaternion -from std_msgs.msg import ColorRGBA +from std_msgs.msg import ColorRGBA, Header from rosgraph_msgs.msg import Clock from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import Pose, PoseStamped, Point +from geometry_msgs.msg import Pose, PoseStamped, Point, Vector3 from visualization_msgs.msg import Marker from navigator_msgs.msg import VehicleControl, VehicleSpeed @@ -180,83 +180,6 @@ def odometry_callback(self, msg: Odometry): def speed_callback(self, msg: VehicleSpeed): self.vehicle_state.velocity = msg.speed - def visualize_waypoint_callback(self): - if self.target_waypoint is not None: - relative_waypoint = 0.4 * np.asarray(self.target_waypoint) - self.publishLookaheadMarker( - self.vehicle_state.lookahead_distance(), relative_waypoint - ) - - def visualize_path_callback(self): - current_path = self.path.get_current_path() - - if len(current_path) == 0: - return - - path_msg = Path() - path_msg.header.frame_id = "base_link" - path_msg.header.stamp = self.clock - - # Each RVIZ grid cell is 0.4m. - path = np.copy(np.asarray(current_path)) * 0.4 - - path_msg.poses = [ - PoseStamped( - header=path_msg.header, - pose=Pose(position=Point(x=x, y=y)), - ) - for x, y in path - ] - - self.lookahead_path_publisher.publish(path_msg) - - def publishLookaheadMarker(self, radius: float, pose: tuple[float, float]): - marker = Marker() - marker.header.frame_id = "base_link" - marker.header.stamp = self.clock - marker.ns = "lookahead" - marker.id = 0 - marker.type = Marker.CYLINDER - - marker.action = Marker.ADD - - marker.scale.x = radius * 2 - marker.scale.y = radius * 2 - marker.scale.z = 0.2 - - color = ColorRGBA() - color.a = 0.3 - color.g = 1.0 - color.b = 1.0 - marker.color = color - - self.barrier_marker_pub.publish(marker) - - marker.id = 1 - marker.type = Marker.ARROW - - marker.action = Marker.ADD - - marker.scale.x = 0.5 - marker.scale.y = 0.8 - marker.scale.z = 0.3 - - color = ColorRGBA() - color.a = 0.7 - color.g = 1.0 - color.b = 0.6 - marker.color = color - - pt_a = Point() - marker.points.append(pt_a) - - pt_b = Point() - pt_b.x = pose[0] - pt_b.y = pose[1] - marker.points.append(pt_b) - - self.barrier_marker_pub.publish(marker) - def control_callback(self): # If no target waypoint, do nothing. if not self.target_waypoint: @@ -283,6 +206,60 @@ def control_callback(self): ) self.command_publisher.publish(control_msg) + def visualize_waypoint_callback(self): + if self.target_waypoint is None: + return + + relative_waypoint = 0.4 * np.asarray(self.target_waypoint) + + lookahead_dist = self.vehicle_state.lookahead_distance() + radius_marker = Marker( + header=Header(frame_id="base_link", stamp=self.clock), + ns="lookahead", + id=0, + type=Marker.CYLINDER, + action=Marker.ADD, + scale=Vector3(x=lookahead_dist * 2, y=lookahead_dist * 2, z=0.2), + color=ColorRGBA(a=0.3, g=1.0, b=1.0), + ) + self.barrier_marker_pub.publish(radius_marker) + + arrow_marker = Marker( + header=Header(frame_id="base_link", stamp=self.clock), + ns="lookahead", + id=1, + type=Marker.ARROW, + action=Marker.ADD, + scale=Vector3(x=0.5, y=0.8, z=0.3), + color=ColorRGBA(a=0.7, g=1.0, b=0.6), + points=[Point(), Point(x=relative_waypoint[0], y=relative_waypoint[1])], + ) + + self.barrier_marker_pub.publish(arrow_marker) + + def visualize_path_callback(self): + current_path = self.path.get_current_path() + + if len(current_path) == 0: + return + + path_msg = Path() + path_msg.header.frame_id = "base_link" + path_msg.header.stamp = self.clock + + # Each RVIZ grid cell is 0.4m. + path = np.copy(np.asarray(current_path)) * 0.4 + + path_msg.poses = [ + PoseStamped( + header=path_msg.header, + pose=Pose(position=Point(x=x, y=y)), + ) + for x, y in path + ] + + self.lookahead_path_publisher.publish(path_msg) + def main(args=None): rclpy.init(args=args) From e4d1270a5e97d99ceccdb2d2e7ebb994b11a64f2 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Wed, 24 Apr 2024 16:34:47 -0500 Subject: [PATCH 17/22] refactor: remove yaw calculation --- .../pure_pursuit_controller_node.py | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 6a83b5df..0fd36075 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -6,7 +6,6 @@ from numpy import typing as npt from rclpy.node import Node -from tf_transformations import euler_from_quaternion from std_msgs.msg import ColorRGBA, Header from rosgraph_msgs.msg import Clock @@ -60,19 +59,7 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: if not self.loaded(): return - _, _, yaw = euler_from_quaternion( - [ - self.pose.orientation.x, - self.pose.orientation.y, - self.pose.orientation.z, - self.pose.orientation.w, - ] - ) - alpha = math.atan2(target[1], target[0]) - self.l.info( - f"Atan2: {math.atan2(target[1], target[0])} Yaw: {yaw} Alpha: {alpha}" - ) delta = math.atan2( 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() ) From c9d94f477f4b6e79c87ce4a5e244100d005f62e2 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Wed, 24 Apr 2024 16:50:58 -0500 Subject: [PATCH 18/22] refactor: comment and clean up ppc --- .../pure_pursuit_controller_node.py | 105 +++++++++++++----- 1 file changed, 80 insertions(+), 25 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 0fd36075..61bbb231 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -27,20 +27,29 @@ class Constants: MAX_THROTTLE = 0.6 # Max speed in m/s MAX_SPEED: float = 1.5 - # Path adjustment gain - PATH_ADJUSTMENT_GAIN: float = 0.5 class VehicleState: - def __init__(self, l): - self.l = l + """Vehicle state class that holds the current pose and velocity of the vehicle.""" + + def __init__(self): self.pose: Pose | None = None self.velocity: float | None = None def loaded(self) -> bool: + """Check if the vehicle state is loaded with pose and velocity. + + Returns: + bool: True if pose and velocity are not None, False otherwise. + """ return self.pose is not None and self.velocity is not None def calc_throttle_brake(self) -> tuple[float, float] | None: + """Calculate throttle and brake based on the current velocity. + + Returns: + tuple[float, float] | None: Throttle and brake values (None if vehicle state is not fulled loaded). + """ if not self.loaded(): return @@ -56,6 +65,14 @@ def calc_throttle_brake(self) -> tuple[float, float] | None: return throttle, brake def calc_steer(self, target: tuple[float, float]) -> float | None: + """Calculate the steering angle based on the target point. + + Args: + target (tuple[float, float]): Target point (x, y) to steer towards. + + Returns: + float | None: Steering angle in the range [-1, 1] (None if vehicle state is not fulled loaded). + """ if not self.loaded(): return @@ -68,23 +85,40 @@ def calc_steer(self, target: tuple[float, float]) -> float | None: return -delta / (math.pi / 2) def lookahead_distance(self) -> float: + """Calculate the lookahead distance based on the current velocity. + + Returns: + float: Lookahead distance in meters. + """ return Constants.kf * self.velocity + Constants.LD class PursuitPath: - def __init__(self, l, path: list[tuple[float, float]] = []): - self.l = l + """Pursuit path class that holds the path and calculates the target point for the vehicle. + + Args: + path (list[tuple[float, float]], optional): List of points (x, y) that represent the path. Defaults to []. + """ + + def __init__(self, path: list[tuple[float, float]] = []): self.path: npt.NDArray[np.float64] = np.asarray(path) - self.prev_index: int | None = None + self.target_index: int | None = None def set_path(self, path: list[tuple[float, float]]) -> None: + """Set the path. + + Args: + path (list[tuple[float, float]]): List of points (x, y) that represent the path. + """ self.path = np.asarray(path) - self.prev_index = None + self.target_index = None - def _calc_nearest_index(self, vs: VehicleState) -> int: - if len(self.path) == 0 or not vs.loaded(): - return + def _calc_nearest_index(self) -> int: + """Calculate the nearest index of the path to the vehicle's current position. + Returns: + int: Nearest index of the path to the vehicle's current position. + """ min_dist = float("inf") min_index = 0 for i, (x, y) in enumerate(self.path): @@ -96,29 +130,43 @@ def _calc_nearest_index(self, vs: VehicleState) -> int: return min_index def get_current_path(self) -> npt.NDArray[np.float64]: - return np.copy(self.path[: self.prev_index]) + """Get the current path up to and including the target point.""" + target_index = self.target_index + 1 if self.target_index is not None else 0 + return np.copy(self.path[:target_index]) def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: + """Calculate the target point based on the vehicle state. + + Args: + vs (VehicleState): Vehicle state object that holds the current pose and velocity of the vehicle. + + Returns: + tuple[float, float] | None: Target point (x, y) to steer towards (None if path is empty or vehicle state is not fulled loaded). + """ if len(self.path) == 0 or not vs.loaded(): return - if self.prev_index is None: - self.prev_index = self._calc_nearest_index(vs) + # If calculating the target point for the first time, find the nearest index to the vehicle's current position. + if self.target_index is None: + self.target_index = self._calc_nearest_index() - for i, (x, y) in enumerate(self.path[self.prev_index :]): + # Find the first point that is further than the lookahead distance. + # If looking at the same path, start from the last target index. + for i, (x, y) in enumerate(self.path[self.target_index :]): dist = math.hypot(x, y) if dist > vs.lookahead_distance(): - self.prev_index = i + self.target_index = i return (x, y) class PursePursuitController(Node): + """Pure pursuit controller node that publishes vehicle control commands based on the pure pursuit algorithm.""" def __init__(self): super().__init__("pure_pursuit_controler") - self.vehicle_state = VehicleState(l=self.get_logger()) - self.path = PursuitPath(l=self.get_logger()) + self.vehicle_state = VehicleState() + self.path = PursuitPath() self.target_waypoint = None self.clock = Clock().clock @@ -138,7 +186,6 @@ def __init__(self): self.command_publisher = self.create_publisher( VehicleControl, "/vehicle/control", 1 ) - self.barrier_marker_pub = self.create_publisher( Marker, "/planning/barrier_marker", 1 ) @@ -153,7 +200,17 @@ def __init__(self): def clock_callback(self, msg: Clock): self.clock = msg.clock + def odometry_callback(self, msg: Odometry): + self.vehicle_state.pose = msg.pose.pose + + def speed_callback(self, msg: VehicleSpeed): + self.vehicle_state.velocity = msg.speed + def route_callback(self, msg: Path): + """Callback function for the path subscriber. + + Recalculates the path and target waypoint based on the received path message. + """ path = [ (pose_stamped.pose.position.x, pose_stamped.pose.position.y) for pose_stamped in msg.poses @@ -161,13 +218,9 @@ def route_callback(self, msg: Path): self.path.set_path(path) self.target_waypoint = self.path.calc_target_point(self.vehicle_state) - def odometry_callback(self, msg: Odometry): - self.vehicle_state.pose = msg.pose.pose - - def speed_callback(self, msg: VehicleSpeed): - self.vehicle_state.velocity = msg.speed - def control_callback(self): + """Calculate and publish the vehicle control commands based on the pure pursuit algorithm.""" + # If no target waypoint, do nothing. if not self.target_waypoint: return @@ -194,6 +247,7 @@ def control_callback(self): self.command_publisher.publish(control_msg) def visualize_waypoint_callback(self): + """Visualize the target waypoint in RVIZ.""" if self.target_waypoint is None: return @@ -225,6 +279,7 @@ def visualize_waypoint_callback(self): self.barrier_marker_pub.publish(arrow_marker) def visualize_path_callback(self): + """Visualize the path in RVIZ.""" current_path = self.path.get_current_path() if len(current_path) == 0: From 64f2e0b4328c9afde1798e89f359bbcdfcbc332c Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Thu, 25 Apr 2024 10:56:40 -0500 Subject: [PATCH 19/22] refactor: delete pure pursuit prototype --- src/control/pure_pursuit/pure_pursuit_node.py | 237 ------------------ 1 file changed, 237 deletions(-) delete mode 100644 src/control/pure_pursuit/pure_pursuit_node.py diff --git a/src/control/pure_pursuit/pure_pursuit_node.py b/src/control/pure_pursuit/pure_pursuit_node.py deleted file mode 100644 index 05a40111..00000000 --- a/src/control/pure_pursuit/pure_pursuit_node.py +++ /dev/null @@ -1,237 +0,0 @@ -# Imports -import os -import time -import math -import rospy2 -import rospkg -import numpy as np -from numpy import linalg as la -import matplotlib.pyplot as plt -from matplotlib import patches -from race.msg import drive_param -from rospkg import RosPack -from nav_msgs.msg import Odometry -from visualization_msgs.msg import Marker -from std_msgs.msg import Header, ColorRGBA -from tf.transformations import euler_from_quaternion, quaternion_from_euler -from geometry_msgs.msg import PoseStamped, Point, Twist, Quaternion, Pose, Point, Vector3 - -# GLOBAL VARIABLES -xc = 0.0 -yc = 0.0 -yaw = 0.0 -vel = 0.0 -idx = 0 -waypoints = [] -v_prev_error = 0.0 -freqs = 10 - -# CAR VARIABLES -# LOOKAHEAD = 1.5 # 1.5 -# WB = 0.3421 -LOOKAHEAD = 0.2 # 1.5 -WB = 0.04 - -# PROGRAM VARIABLES -pure_pursuit_flag = True -show_animation = True - -# PID Logic inferred from rtp_node.py -# TODO -# Update to newer PID logic and check for target_speed unit -# Fine tuning -def calculate_throttle(current_speed, target_speed): - #target_speed = 1.5 # MAX_SPEED # m/s, ~10mph - - # Max speed check - if(target_speed > 1.5): - target_speed = 1.5 - - pid_error = target_speed - current_speed - # self.get_logger().info(f"Steer/Pose: {command.steer}/{((best_path.poses[4][2] + best_path.poses[5][2] + best_path.poses[6][2])/3.0)}") - # print(f"Speed: {self.speed} / {target_speed}") - if pid_error > 0: - command.throttle = min(pid_error *0.5, 0.6) - command.brake = 0.0 - #else: - elif pid_error <= -1: - brake = pid_error *0.6 *-1.0 - throttle = 0.0 - - return throttle, brake - -# TODO -# Set path subscriber here -# /planning/path -def path_callback(data): - """ - CHANGE THIS PATH TO WHERE YOU HAVE SAVED YOUR CSV FILES - """ - # r = rospkg.RosPack() - # package_path = r.get_path('pure_pursuit') - # file_name = 'wp_file.csv' #'racecar_walker.csv' - # file_path = package_path + '/waypoints/' + file_name - # with open(file_path) as f: - # path_points = np.loadtxt(file_path, delimiter = ',') - # return path_points - - waypoints = data.poses - -def pose_callback(data): - """ - Get current state of the vehicle - """ - global xc, yc, yaw, vel - xc = data.pose.pose.position.x - yc = data.pose.pose.position.y - - # Convert Quaternions to Eulers - qx = data.pose.pose.orientation.x - qy = data.pose.pose.orientation.y - qz = data.pose.pose.orientation.z - qw = data.pose.pose.orientation.w - quaternion = (qx,qy,qz,qw) - euler = euler_from_quaternion(quaternion) - yaw = euler[2] - vel = la.norm(np.array([data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z]),2) - -def find_distance(x1, y1): - global xc, yc, yaw, waypoints - distance = math.sqrt((x1 - xc) ** 2 + (y1 - yc) ** 2) - return distance - -def find_distance_index_based(idx): - global xc, yc, yaw, waypoints - x1 = float(waypoints[idx][0]) - y1 = float(waypoints[idx][1]) - distance = math.sqrt((x1 - xc) ** 2 + (y1 - yc) ** 2) - return distance - -def find_nearest_waypoint(): - """ - Get closest idx to the vehicle - """ - global xc, yc, yaw, waypoints, WB - curr_xy = np.array([xc, yc]) - waypoints_xy = waypoints[:, :2] - nearest_idx = np.argmin(np.sum((curr_xy - waypoints_xy)**2, axis=1)) - return nearest_idx - -def idx_close_to_lookahead(idx): - """ - Get closest index to lookahead that is greater than the lookahead - """ - global LOOKAHEAD - while find_distance_index_based(idx) < LOOKAHEAD: - idx += 1 - return idx - 1 - -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): - """ - Plot arrow - """ - if not isinstance(x, float): - for ix, iy, iyaw in zip(x, y, yaw): - plot_arrow(ix, iy, iyaw) - else: - plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc, ec=ec, head_width=width, head_length=width) - plt.plot(x, y) - patches.Rectangle((xc,yc), 0.35,0.2) - -def pure_pursuit(): - global xc, yc, yaw, vel, waypoints, v_prev_error, freqs, idx,LOOKAHEAD, WB, pure_pursuit_flag, show_animation - - # Initialize the message, subscriber and publisher - msg = Twist() - - rospy.Subscriber("/gnss/odometry", Odometry, pose_callback) - rospy.Subscriber("/planning/path", Path, path_callback) - pub = rospy.Publisher('/vehicle/control', Twist, queue_size=1) - - cx = waypoints[:, 0]; cy = waypoints[:, 1] - - try: - while pure_pursuit_flag: - nearest_idx = find_nearest_waypoint() - idx_near_lookahead = idx_close_to_lookahead(nearest_idx) - target_x = float(waypoints[idx_near_lookahead][0]) - target_y = float(waypoints[idx_near_lookahead][1]) - - # Velocity PID controller - kp = 1.0 # 1.0 - kd = 0 - ki = 0 - - dt = 1.0 / freqs - v_desired = float(waypoints[nearest_idx][3]) - v_error = v_desired - vel - - P_vel = kp * v_error - I_vel = v_error * dt - D_vel = kd * (v_error - v_prev_error) / dt - - velocity = P_vel + I_vel + D_vel - v_prev_error = v_error - print(f"NEAREST INDEX = {nearest_idx}, output = {velocity}, velocity desired = {v_desired}, current = {vel}") - - # Setting variables to publish - throttle, brake = calculate_throttle(vel, v_desired) - - # PURE PURSUIT CONTROLLER - - # calculate alpha (angle between the goal point and the path point) - x_delta = target_x - xc - y_delta = target_y - yc - alpha = np.arctan(y_delta / x_delta) - yaw - - # front of the vehicle is 0 degrees right +90 and left -90 hence we need to convert our alpha - if alpha > np.pi / 2: - alpha -= np.pi - if alpha < -np.pi / 2: - alpha += np.pi - - # Set the lookahead distance depending on the speed - lookahead = find_distance(target_x, target_y) - steering_angle = np.arctan((2 * WB * np.sin(alpha)) / lookahead) - - # Set max wheel turning angle - if steering_angle > 0.5: - steering_angle = 0.5 - elif steering_angle < -0.5: - steering_angle = -0.5 - - # Publish messages - msg.Steer = steering_angle - msg.Throttle = throttle - msg.Brake = brake - pub.publish(msg) - - # Plot map progression - if show_animation: - plt.cla() - # For stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - plot_arrow(float(xc), float(yc), float(yaw)) - plt.plot(cx, cy, "-r", label = "course") - plt.plot(xc, yc, "-b", label = "trajectory") - plt.plot(target_x, target_y, "xg", label = "target") - plt.axis("equal") - plt.grid(True) - plt.title("Pure Pursuit Control" + str(1)) - plt.pause(0.001) - - except IndexError: - print("PURE PURSUIT COMPLETE --> COMPLETED ALL WAYPOINTS") - - - -if __name__=='__main__': - rospy.init_node('pure_pursuit') - r = rospy.Rate(freqs) - print("RUNNING PURE-PURSUIT CODE.... \n\n") - time.sleep(2) - pure_pursuit() - -# path message (points x y) -> Steering angle, desired velocity -# pid -> desired - From 4418b1d0491c98352d8d3e6a530150575d577a45 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Thu, 25 Apr 2024 11:46:29 -0500 Subject: [PATCH 20/22] feat: merge nav2 rviz configs with ppc --- data/navigator_default.rviz | 175 +++++++++++++++++++++++++++++------- 1 file changed, 143 insertions(+), 32 deletions(-) diff --git a/data/navigator_default.rviz b/data/navigator_default.rviz index 811b6f2c..9325104c 100644 --- a/data/navigator_default.rviz +++ b/data/navigator_default.rviz @@ -4,12 +4,13 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /TF1/Frames1 - /Routes Paths1 - - /Cameras1 + - /Routes Paths1/Path1/Topic1 - /Grids1 - - /Marker1 - - /Marker1/Status1 + - /Nav21/Map1 + - /Pure Pursuit Controller1 Splitter Ratio: 0.5 Tree Height: 603 - Class: rviz_common/Selection @@ -57,34 +58,20 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description - Enabled: true + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hero: - Alpha: 1 - Show Axes: false - Show Trail: false - model_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Class: rviz_default_plugins/TF Enabled: false @@ -183,7 +170,7 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable + Reliability Policy: Best Effort Value: /planning/path Value: true Enabled: true @@ -239,9 +226,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.9904967546463013 + Max Intensity: 0.9850215315818787 Min Color: 0; 73; 112 - Min Intensity: 0.7124737501144409 + Min Intensity: 0.716873288154602 Name: Filtered LiDAR Position Transformer: XYZ Selectable: true @@ -338,7 +325,7 @@ Visualization Manager: Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map - Color Scheme: raw + Color Scheme: map Draw Behind: true Enabled: false Name: Drivable Area @@ -541,17 +528,141 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/Marker Enabled: true - Name: Marker + Name: Planning Goal + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/goal_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Planning Look Ahead Namespaces: - lookahead: true + "": true Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/barrier_marker + Value: /planning/look_ahead_marker Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/global_costmap/published_footprint + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/plan + Value: true + Enabled: true + Name: Nav2 + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 1 + Name: Pure Pursuit Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ppc/path + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Pure Pursuit Lookahead + Namespaces: + lookahead: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/barrier_marker + Value: true + Enabled: true + Name: Pure Pursuit Controller Enabled: true Global Options: Background Color: 222; 222; 222 @@ -598,7 +709,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 40.92072296142578 + Distance: 20.80792236328125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -613,10 +724,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.649796724319458 + Pitch: 0.9497964382171631 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 2.9573850631713867 + Yaw: 2.19738507270813 Saved: ~ Window Geometry: Birds Eye: @@ -634,13 +745,13 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1846 - X: 74 + Width: 1310 + X: 610 Y: 27 From 79ce489206c84662eafa9f20b7e768ae288d0aaf Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Thu, 25 Apr 2024 11:48:55 -0500 Subject: [PATCH 21/22] refactor: remove rtp changes (a single space) from PR changes and rm unsued test files --- .../test/test_copyright.py | 25 ------------------- .../test/test_flake8.py | 25 ------------------- .../test/test_pep257.py | 23 ----------------- src/planning/rtp/rtp/rtp_node.py | 2 +- 4 files changed, 1 insertion(+), 74 deletions(-) delete mode 100755 src/control/pure_pursuit_controller/test/test_copyright.py delete mode 100755 src/control/pure_pursuit_controller/test/test_flake8.py delete mode 100755 src/control/pure_pursuit_controller/test/test_pep257.py diff --git a/src/control/pure_pursuit_controller/test/test_copyright.py b/src/control/pure_pursuit_controller/test/test_copyright.py deleted file mode 100755 index 97a39196..00000000 --- a/src/control/pure_pursuit_controller/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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/pure_pursuit_controller/test/test_flake8.py b/src/control/pure_pursuit_controller/test/test_flake8.py deleted file mode 100755 index 27ee1078..00000000 --- a/src/control/pure_pursuit_controller/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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/pure_pursuit_controller/test/test_pep257.py b/src/control/pure_pursuit_controller/test/test_pep257.py deleted file mode 100755 index b234a384..00000000 --- a/src/control/pure_pursuit_controller/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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' diff --git a/src/planning/rtp/rtp/rtp_node.py b/src/planning/rtp/rtp/rtp_node.py index 64d6778a..0a22af42 100755 --- a/src/planning/rtp/rtp/rtp_node.py +++ b/src/planning/rtp/rtp/rtp_node.py @@ -536,7 +536,7 @@ def costMapCb(self, msg: OccupancyGrid): if np.sqrt(pose[0]**2 + pose[1]**2) > lookahead_distance: lookahead_pose = pose break - + if lookahead_pose is None: lookahead_pose = poses_np_bl[-1] From a6569742e83119a0cec6286baef902a21a3d0092 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Thu, 25 Apr 2024 18:23:02 -0500 Subject: [PATCH 22/22] feat: add distance/brake proportion --- .../pure_pursuit_controller_node.py | 43 ++++++++++++++++++- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py index 61bbb231..9c38e0b1 100755 --- a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -24,9 +24,14 @@ class Constants: # Wheel base (distance between front and rear wheels) in meter WHEEL_BASE: float = 3.5 # Max throttle and acceleration - MAX_THROTTLE = 0.6 + MAX_THROTTLE = 2 # Max speed in m/s MAX_SPEED: float = 1.5 + # Stopping distance in meters + # This is the distance from the end of the path at which the vehicle will start to brake. + STOPPING_DIST: float = 10 + # Max break possible + MAX_BREAK: float = 1.0 class VehicleState: @@ -104,6 +109,13 @@ def __init__(self, path: list[tuple[float, float]] = []): self.path: npt.NDArray[np.float64] = np.asarray(path) self.target_index: int | None = None + def distance(self) -> float: + if len(self.path) == 0: + return 0.0 + + x, y = self.path[-1] + return math.hypot(x, y) + def set_path(self, path: list[tuple[float, float]]) -> None: """Set the path. @@ -150,6 +162,11 @@ def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: if self.target_index is None: self.target_index = self._calc_nearest_index() + # When the path is shorter then the lookahead, we will pick the last point. + if self.distance() < vs.lookahead_distance(): + x, y = self.path[-1] + return (x, y) + # Find the first point that is further than the lookahead distance. # If looking at the same path, start from the last target index. for i, (x, y) in enumerate(self.path[self.target_index :]): @@ -216,15 +233,37 @@ def route_callback(self, msg: Path): for pose_stamped in msg.poses ] self.path.set_path(path) - self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + + def stop_vehicle(self, steer: float, break_value: float): + """Stop the vehicle by applying the break.""" + control_msg = VehicleControl() + control_msg.brake = break_value + control_msg.steer = steer + self.command_publisher.publish(control_msg) def control_callback(self): """Calculate and publish the vehicle control commands based on the pure pursuit algorithm.""" + # Calculate the waypoint just outside the lookahead distance. + self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + self.get_logger().info(f"Target Waypoint: {self.target_waypoint}") # If no target waypoint, do nothing. if not self.target_waypoint: return + path_dist = self.path.distance() + if path_dist < Constants.STOPPING_DIST: + # Scale the break value linearly based on the distance to the end of the path. + # Break scale is 1.0 at the end of the path and 0.0 at the stopping distance. + break_scale = 1.0 - (path_dist / Constants.STOPPING_DIST) + break_value = min( + Constants.MAX_BREAK, + break_scale * Constants.MAX_BREAK, + ) + steer = self.vehicle_state.calc_steer(self.target_waypoint) + self.stop_vehicle(steer, break_value) + return + # Calculate throttle, brake, and steer throttle_brake = self.vehicle_state.calc_throttle_brake() steer = self.vehicle_state.calc_steer(self.target_waypoint)