From 24f30800a42f5889ed3013e48f84dfc76e172a20 Mon Sep 17 00:00:00 2001 From: prajwal1121 Date: Wed, 6 Apr 2022 21:13:26 -0400 Subject: [PATCH 1/6] Added closest segment math --- src/pure_pursuit.py | 64 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 60 insertions(+), 4 deletions(-) diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py index e4d1923..fab89ad 100755 --- a/src/pure_pursuit.py +++ b/src/pure_pursuit.py @@ -11,27 +11,83 @@ from ackermann_msgs.msg import AckermannDriveStamped from nav_msgs.msg import Odometry +from scipy.spatial.transform import Rotation as R + class PurePursuit(object): """ Implements Pure Pursuit trajectory tracking with a fixed lookahead and speed. """ def __init__(self): self.odom_topic = rospy.get_param("~odom_topic") - self.lookahead = # FILL IN # - self.speed = # FILL IN # - self.wrap = # FILL IN # - self.wheelbase_length = # FILL IN # + self.lookahead = rospy.get_param("lookahead",0.5) + self.speed = rospy.get_param("speed",1.0) + #self.wrap = # FILL IN # + self.wheelbase_length = 0.568 + self.base_link_offset = rospy.get_param("base_link_offset",0.1) + self.robot_pose = None self.trajectory = utils.LineTrajectory("/followed_trajectory") self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1) + self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.pose_callback, queue_size = 1) self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=1) def trajectory_callback(self, msg): ''' Clears the currently followed trajectory, and loads the new one from the message ''' print "Receiving new trajectory:", len(msg.poses), "points" + self.trajectory.clear() self.trajectory.fromPoseArray(msg) self.trajectory.publish_viz(duration=0.0) + + # + # Find nearest line segment + # + segments = np.array(self.trajectory.points()) + num_segments = segments.shape[0] + robot_point = np.tile(self.robot_pose[:2],(num_segments-1,1)) + + segment_start = segments[:-1,:] + segment_end = segments[1:,:] + + #Vectorize + robot_vector = robot_point-segment_start + segment_vector = segment_end-segment_start + + #Normalize and Scale + segment_length = np.linalg.norm(segment_vector, axis=1) + segment_length = np.reshape(segment_length, (num_segments-1,1)) + + unit_segment = np.divide(segment_vector,segment_length) + scaled_robot_point = np.divide(robot_vector,segment_length) + + #Project + scaled_dot_product = np.sum(unit_segment*scaled_robot_point, axis=1) + #Clip and Rescale + scaled_dot_product = np.clip(scaled_dot_product,0.0,1.0) + scaled_dot_product = np.reshape(scaled_dot_product, (num_segments-1,1)) + + scaled_segment = segment_vector*scaled_dot_product + + #Offset + shifted = np.add(scaled_segment,segment_start) + + #Find Distances + shortest_distance = np.linalg.norm(shifted-robot_point, axis=1) + + #Grab relevant segment + segment_index = np.argmin(shortest_distance) + nearest_segment_start = segments[segment_index] + nearest_segment_end = segments[segment_index+1] + + def pose_callback(self, msg): + ''' Clears the currently followed trajectory, and loads the new one from the message + ''' + pose = [msg.pose.pose.position.x+self.base_link_offset,msg.pose.pose.position.y] + r = R.from_quat([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + theta = r.as_euler('XYZ')[2] + pose.append(theta) + self.robot_pose = np.array(pose) + if __name__=="__main__": rospy.init_node("pure_pursuit") From 7bf672bc889dca21be5b4d41caaa762d6a93f694 Mon Sep 17 00:00:00 2001 From: prajwal1121 Date: Fri, 8 Apr 2022 16:53:01 -0400 Subject: [PATCH 2/6] Added circle intersection --- src/pure_pursuit.py | 76 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 69 insertions(+), 7 deletions(-) diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py index fab89ad..6e1a0d0 100755 --- a/src/pure_pursuit.py +++ b/src/pure_pursuit.py @@ -29,6 +29,9 @@ def __init__(self): self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.pose_callback, queue_size = 1) self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=1) + self.segment_index = None + self.goal_point = None + def trajectory_callback(self, msg): ''' Clears the currently followed trajectory, and loads the new one from the message ''' @@ -38,6 +41,67 @@ def trajectory_callback(self, msg): self.trajectory.fromPoseArray(msg) self.trajectory.publish_viz(duration=0.0) + if robot_pose is not None: + self.segment_index = self.find_nearest_segment() + self.goal_point = self.find_circle_interesection() + + if self.goal_point is None: + #Case 3 no intersections. What do + pass + + def find_circle_interesection(self): + # + # Find Circle Intersection + # Start at nearest segment and search next three segments + # + search_end_index = self.segment_index+3 + if search_end_index > segments.shape[0]-1: + search_end_index = segments.shape[0]-1 + + goal_point = None + + #Check nearest segment as well as a couple segments up. Choose the furthest goal point + for i in xrange(self.segment_index,search_end_index) + P1 = segments[segment_index] + P2 = segments[segment_index+1] + Q = self.robot_pose[:2] + r = self.lookahead + V = P2-P1 + + a = V.dot(V) + b = 2.*V.dot(P1-Q) + c = P1.dot(P1)+Q.dot(Q)-2.*P1.dot(Q)-r**2. + + disc = b**2.-4.*a*c + + if disc < 0.: + #No intersection at all + pass + else: + #Two solutions + sqrt_disc = np.sqrt(disc) + t1 = (-b + sqrt_disc) / (2. * a) + t2 = (-b - sqrt_disc) / (2. * a) + + #Choose the solution that is actually on the line segmnet (0 <= t <= 1) + if ((t1 < 0. or t1 > 1.) and (t2 < 0. or t2 > 1.)): + #No intersection on segment + pass + elif ((t1 >= 0. or t1 <= 1.) and (t2 < 0. or t2 > 1.)): + goal_point = P1+t1*V + elif ((t1 < 0. or t1 > 1.) and (t2 >= 0. or t2 <= 1.)): + goal_point = P1+t2*V + #Otherwise choose the one closest to the next point + else: + intersects = [P1+t1*V,P1+t2*V] + + dist_P2_intersects = np.array([np.linalg.norm(P2-intersects[0]),np.linalg.norm(P2-intersects[1])]) + goal_point = intersects[np.argmin(dist_P2_intersects)] + + #Return intersection. (None if no intersections) + return goal_point + + def find_nearest_segment(self): # # Find nearest line segment # @@ -51,17 +115,17 @@ def trajectory_callback(self, msg): #Vectorize robot_vector = robot_point-segment_start segment_vector = segment_end-segment_start - + #Normalize and Scale segment_length = np.linalg.norm(segment_vector, axis=1) segment_length = np.reshape(segment_length, (num_segments-1,1)) - + unit_segment = np.divide(segment_vector,segment_length) scaled_robot_point = np.divide(robot_vector,segment_length) - + #Project scaled_dot_product = np.sum(unit_segment*scaled_robot_point, axis=1) - + #Clip and Rescale scaled_dot_product = np.clip(scaled_dot_product,0.0,1.0) scaled_dot_product = np.reshape(scaled_dot_product, (num_segments-1,1)) @@ -75,9 +139,7 @@ def trajectory_callback(self, msg): shortest_distance = np.linalg.norm(shifted-robot_point, axis=1) #Grab relevant segment - segment_index = np.argmin(shortest_distance) - nearest_segment_start = segments[segment_index] - nearest_segment_end = segments[segment_index+1] + return(np.argmin(shortest_distance)) def pose_callback(self, msg): ''' Clears the currently followed trajectory, and loads the new one from the message From b6b68ea574b66246ed0f9083719ec35f2a229f1b Mon Sep 17 00:00:00 2001 From: prajwal1121 Date: Fri, 8 Apr 2022 16:58:43 -0400 Subject: [PATCH 3/6] Added segment lookahead --- src/pure_pursuit.py | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py index 6e1a0d0..b31ff94 100755 --- a/src/pure_pursuit.py +++ b/src/pure_pursuit.py @@ -17,18 +17,25 @@ class PurePursuit(object): """ Implements Pure Pursuit trajectory tracking with a fixed lookahead and speed. """ def __init__(self): - self.odom_topic = rospy.get_param("~odom_topic") + self.lookahead = rospy.get_param("lookahead",0.5) self.speed = rospy.get_param("speed",1.0) - #self.wrap = # FILL IN # - self.wheelbase_length = 0.568 self.base_link_offset = rospy.get_param("base_link_offset",0.1) - self.robot_pose = None + self.wheelbase_length = 0.568 + #self.wrap = # FILL IN # + self.trajectory = utils.LineTrajectory("/followed_trajectory") self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1) - self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.pose_callback, queue_size = 1) + + self.robot_pose = None + self.odom_topic = rospy.get_param("~odom_topic") + self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.pose_callback, queue_size = 1) + self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=1) - + + #How many segments to search ahead of the nearest + self.segment_lookahead = 3 + self.segment_index = None self.goal_point = None @@ -54,7 +61,7 @@ def find_circle_interesection(self): # Find Circle Intersection # Start at nearest segment and search next three segments # - search_end_index = self.segment_index+3 + search_end_index = self.segment_index+self.segment_lookahead if search_end_index > segments.shape[0]-1: search_end_index = segments.shape[0]-1 From ab51889072d9562e89726e2adb0bad79c0613c06 Mon Sep 17 00:00:00 2001 From: prajwal1121 Date: Fri, 8 Apr 2022 17:02:52 -0400 Subject: [PATCH 4/6] Fixed syntax errors --- src/pure_pursuit.py | 39 ++++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py index b31ff94..88d4c0f 100755 --- a/src/pure_pursuit.py +++ b/src/pure_pursuit.py @@ -36,19 +36,21 @@ def __init__(self): #How many segments to search ahead of the nearest self.segment_lookahead = 3 + self.segments = None self.segment_index = None self.goal_point = None def trajectory_callback(self, msg): ''' Clears the currently followed trajectory, and loads the new one from the message ''' - print "Receiving new trajectory:", len(msg.poses), "points" + print("Receiving new trajectory:", len(msg.poses), "points") self.trajectory.clear() self.trajectory.fromPoseArray(msg) self.trajectory.publish_viz(duration=0.0) + self.segments = np.array(self.trajectory.points()) - if robot_pose is not None: + if self.robot_pose is not None: self.segment_index = self.find_nearest_segment() self.goal_point = self.find_circle_interesection() @@ -62,15 +64,15 @@ def find_circle_interesection(self): # Start at nearest segment and search next three segments # search_end_index = self.segment_index+self.segment_lookahead - if search_end_index > segments.shape[0]-1: - search_end_index = segments.shape[0]-1 + if search_end_index > self.segments.shape[0]-1: + search_end_index = self.segments.shape[0]-1 goal_point = None #Check nearest segment as well as a couple segments up. Choose the furthest goal point - for i in xrange(self.segment_index,search_end_index) - P1 = segments[segment_index] - P2 = segments[segment_index+1] + for i in xrange(self.segment_index,search_end_index): + P1 = self.segments[self.segment_index] + P2 = self.segments[self.segment_index+1] Q = self.robot_pose[:2] r = self.lookahead V = P2-P1 @@ -112,12 +114,11 @@ def find_nearest_segment(self): # # Find nearest line segment # - segments = np.array(self.trajectory.points()) - num_segments = segments.shape[0] + num_segments = self.segments.shape[0] robot_point = np.tile(self.robot_pose[:2],(num_segments-1,1)) - segment_start = segments[:-1,:] - segment_end = segments[1:,:] + segment_start = self.segments[:-1,:] + segment_end = self.segments[1:,:] #Vectorize robot_vector = robot_point-segment_start @@ -148,14 +149,14 @@ def find_nearest_segment(self): #Grab relevant segment return(np.argmin(shortest_distance)) - def pose_callback(self, msg): - ''' Clears the currently followed trajectory, and loads the new one from the message - ''' - pose = [msg.pose.pose.position.x+self.base_link_offset,msg.pose.pose.position.y] - r = R.from_quat([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) - theta = r.as_euler('XYZ')[2] - pose.append(theta) - self.robot_pose = np.array(pose) + def pose_callback(self, msg): + ''' Clears the currently followed trajectory, and loads the new one from the message + ''' + pose = [msg.pose.pose.position.x+self.base_link_offset,msg.pose.pose.position.y] + r = R.from_quat([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + theta = r.as_euler('XYZ')[2] + pose.append(theta) + self.robot_pose = np.array(pose) if __name__=="__main__": From 34f8d4acb2d8c9e46724d4e5312b06a472a64af2 Mon Sep 17 00:00:00 2001 From: spkhowe Date: Sun, 10 Apr 2022 22:12:02 +0000 Subject: [PATCH 5/6] drive at calculated driving angle --- launch/load_trajectory.launch | 2 +- src/pure_pursuit.py | 36 +++++++++++++++++++++++++---------- trajectories/test_traj.traj | 1 + 3 files changed, 28 insertions(+), 11 deletions(-) create mode 100644 trajectories/test_traj.traj diff --git a/launch/load_trajectory.launch b/launch/load_trajectory.launch index 61ff31e..31179ae 100644 --- a/launch/load_trajectory.launch +++ b/launch/load_trajectory.launch @@ -1,5 +1,5 @@ - + diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py index 88d4c0f..9eb3b68 100755 --- a/src/pure_pursuit.py +++ b/src/pure_pursuit.py @@ -10,7 +10,6 @@ from visualization_msgs.msg import Marker from ackermann_msgs.msg import AckermannDriveStamped from nav_msgs.msg import Odometry - from scipy.spatial.transform import Rotation as R class PurePursuit(object): @@ -18,24 +17,22 @@ class PurePursuit(object): """ def __init__(self): - self.lookahead = rospy.get_param("lookahead",0.5) - self.speed = rospy.get_param("speed",1.0) - self.base_link_offset = rospy.get_param("base_link_offset",0.1) + self.lookahead = rospy.get_param("lookahead", 0.5) + self.speed = rospy.get_param("speed", 1.0) + self.base_link_offset = rospy.get_param("base_link_offset", 0.1) self.wheelbase_length = 0.568 - #self.wrap = # FILL IN # self.trajectory = utils.LineTrajectory("/followed_trajectory") - self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1) - + # self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1) + self.traj_sub = rospy.Subscriber("/loaded_trajectory/path", PoseArray, self.trajectory_callback, queue_size=1) # for simulation i tink... + self.robot_pose = None self.odom_topic = rospy.get_param("~odom_topic") self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.pose_callback, queue_size = 1) - self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=1) #How many segments to search ahead of the nearest self.segment_lookahead = 3 - self.segments = None self.segment_index = None self.goal_point = None @@ -53,10 +50,29 @@ def trajectory_callback(self, msg): if self.robot_pose is not None: self.segment_index = self.find_nearest_segment() self.goal_point = self.find_circle_interesection() + + x1, y1 = self.robot_pose + x2, y2 = self.goal_point # ok this is actually in map frame so find distance using robot pose + x = abs(x2-x1) + y = abs(y2-y1) + theta = np.arctan(y/x) # bearing + d = np.sqrt(x**2 + y**2) # euclidean distance + delta = np.arctan((2*self.wheelbase_length*np.sin(theta))/d) # steering angle if self.goal_point is None: #Case 3 no intersections. What do + # maybe increase lookahead circle until goal_point exists? + # might not be very safe though for avoidance of obstacles, etc. pass + + # Drive to goal point + drive_msg = AckermannDriveStamped() + drive_msg.drive.steering_angle = delta + drive_msg.drive.steering_angle_velocity = 0 + drive_msg.drive.speed = self.speed + drive_msg.drive.acceleration = 0 + drive_msg.drive.jerk = 0 + self.drive_pub.publish(drive_msg) def find_circle_interesection(self): # @@ -70,7 +86,7 @@ def find_circle_interesection(self): goal_point = None #Check nearest segment as well as a couple segments up. Choose the furthest goal point - for i in xrange(self.segment_index,search_end_index): + for i in range(self.segment_index,search_end_index): P1 = self.segments[self.segment_index] P2 = self.segments[self.segment_index+1] Q = self.robot_pose[:2] diff --git a/trajectories/test_traj.traj b/trajectories/test_traj.traj new file mode 100644 index 0000000..fbf8cb0 --- /dev/null +++ b/trajectories/test_traj.traj @@ -0,0 +1 @@ +{"points": [{"y": 0.07693011313676834, "x": 1.9834070205688477}, {"y": 2.2620317935943604, "x": 3.845733165740967}, {"y": 5.787002086639404, "x": 4.032938480377197}, {"y": 9.36202335357666, "x": 2.7570769786834717}]} \ No newline at end of file From 671a8456a9d17c75b2fc051c1d0093a8c932e3ad Mon Sep 17 00:00:00 2001 From: spkhowe Date: Tue, 12 Apr 2022 00:51:29 +0000 Subject: [PATCH 6/6] pure pursuit working :D --- .DS_Store | Bin 0 -> 6148 bytes launch/follow_trajectory.launch | 2 +- src/.DS_Store | Bin 0 -> 6148 bytes src/pure_pursuit.py | 97 +++++++++++++++++++------------- src/visualization_tools.py | 42 ++++++++++++++ 5 files changed, 102 insertions(+), 39 deletions(-) create mode 100644 .DS_Store create mode 100644 src/.DS_Store create mode 100644 src/visualization_tools.py diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..55b3807e3af67968016c28e432fdc57ac0ad4341 GIT binary patch literal 6148 zcmeHKL2uJA82#L4TS@`-0D=orB(75_uuYS=gswdx!DT^k092Bu+CtN~YEp_+Rq7S^ z5Bvp={1g5QCwQOjNlgYuYa$Zed3>L!MMN$NqjLq-Z-U3UT#B0M zVVzLd7|~NoD5nC&hIPO?ucl=e>* z$cT0+g>p_A4e7kX{aWEVGy!)?d6_5fRCqh!jVQ)S5>}X>E6gUY7UIrh)MwDN@x&8K zE2pnCSipz@HSw&IDdN!kBnWen=vf+hcMT>lD9Qzhc(w|~F=o?&XR7(dcbp!x9=vIN8|0b};Gw2&!Ys3gbXem%jg*jpf zEl0m+c)r24MlB~{4j;nYS(p=wPz&cQNpst%O-v9f5KL3|Rwq+f# z4*XXR2&WhHx|ovLTQ5zH_gW9-EeZ$o)*6)rmD!F}!dvle6crfrI05 - + diff --git a/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..3c4e1584388fb42093816516d103e98a1e991e1c GIT binary patch literal 6148 zcmeHKOHKko5UoNMs0&&6VPi5UAn^tx!G&uNpfDI5GhqmV39e@E9lV0)bEU7UItH4- z#EmgpMXFzas$Ne&($mvK#J$DnjHpdSEx2HF4@Hg1xcG)OLXQlxa$M7t3L4M^KQ~vv z75ME6*yf*N&(7(NR`+j?Jv^o^cJ%VHD6?Tv4zU<{QJllO?`pcd1H>$7Mz@+TwFk6w zFxF+iM%hP*n_>QYxwnTQN}Snf?re`V3wSoRR0{$Il;