diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000..55b3807
Binary files /dev/null and b/.DS_Store differ
diff --git a/launch/follow_trajectory.launch b/launch/follow_trajectory.launch
index bbb8705..bd392dc 100644
--- a/launch/follow_trajectory.launch
+++ b/launch/follow_trajectory.launch
@@ -1,5 +1,5 @@
-
+
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/.DS_Store b/src/.DS_Store
new file mode 100644
index 0000000..3c4e158
Binary files /dev/null and b/src/.DS_Store differ
diff --git a/src/pure_pursuit.py b/src/pure_pursuit.py
index 9550c53..46bde5b 100755
--- a/src/pure_pursuit.py
+++ b/src/pure_pursuit.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python
+from tokenize import String
import rospy
import numpy as np
import time
@@ -10,27 +11,189 @@
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
+from visualization_tools import *
+
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.wheelbase_length = # FILL IN #
+ self.lookahead = rospy.get_param("lookahead", 0.8)
+ self.speed = rospy.get_param("speed", 1.0)
+ self.base_link_offset = rospy.get_param("base_link_offset", 0.0)
+ self.wheelbase_length = 0.568
+
self.trajectory = utils.LineTrajectory("/followed_trajectory")
- self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
- self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, 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)
+ self.goal_pub = rospy.Publisher("/goal", Marker, 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
+
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)
+
+ def find_circle_interesection(self):
+ #
+ # Find Circle Intersection
+ # Start at nearest segment and search next three segments
+ #
+ search_end_index = self.segment_index+self.segment_lookahead
+ 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 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]
+ 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
+ #
+ num_segments = self.segments.shape[0]
+ robot_point = np.tile(self.robot_pose[:2],(num_segments-1,1))
+
+ segment_start = self.segments[:-1,:]
+ segment_end = self.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
+ 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)
+
+ if self.segments is not None:
+ self.segment_index = self.find_nearest_segment()
+ self.goal_point = self.find_circle_interesection()
+
+ if self.goal_point is None:
+ self.goal_point = self.segments[self.segment_index + 1]
+ VisualizationTools.plot_line([self.goal_point[0], self.goal_point[0] + .01], [self.goal_point[1], self.goal_point[1] + .01], self.goal_pub, frame="/map")
+
+
+ x_c, y_c, theta_c = self.robot_pose
+ x_g, y_g = self.goal_point
+ T_R_W = np.array([[ np.cos(theta_c), -np.sin(theta_c), x_c ],
+ [ np.sin(theta_c), np.cos(theta_c), y_c ],
+ [ 0, 0, 1 ]])
+
+ T_G_W = np.array([[ np.cos(0), -np.sin(0), x_g ],
+ [ np.sin(0), np.cos(0), y_g ],
+ [ 0, 0, 1 ]])
+
+ T_G_R = np.dot(np.linalg.inv(T_R_W), T_G_W)
+
+ x = T_G_R[0][2]
+ y = T_G_R[1][2]
+
+ 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:
+ pass
+ # Drive to goal point
+ drive_msg = AckermannDriveStamped()
+ drive_msg.header.stamp = rospy.get_rostime()
+ drive_msg.header.frame_id = "base_link"
+ drive_msg.drive.steering_angle = delta
+ drive_msg.drive.steering_angle_velocity = 0
+ if self.segments is None:
+ drive_msg.drive.speed = 0
+ else:
+ drive_msg.drive.speed = self.speed
+ drive_msg.drive.acceleration = 0
+ drive_msg.drive.jerk = 0
+ self.drive_pub.publish(drive_msg)
+
if __name__=="__main__":
rospy.init_node("pure_pursuit")
diff --git a/src/visualization_tools.py b/src/visualization_tools.py
new file mode 100644
index 0000000..5f570ec
--- /dev/null
+++ b/src/visualization_tools.py
@@ -0,0 +1,42 @@
+from geometry_msgs.msg import Point
+from visualization_msgs.msg import Marker
+
+class VisualizationTools:
+
+ @staticmethod
+ def plot_line(x, y, publisher, color = (1., 0., 0.), frame = "/map"):
+ """
+ Publishes the points (x, y) to publisher
+ so they can be visualized in rviz as
+ connected line segments.
+ Args:
+ x, y: The x and y values. These arrays
+ must be of the same length.
+ publisher: the publisher to publish to. The
+ publisher must be of type Marker from the
+ visualization_msgs.msg class.
+ color: the RGB color of the plot.
+ frame: the transformation frame to plot in.
+ """
+ # Construct a line
+ line_strip = Marker()
+ line_strip.type = Marker.LINE_STRIP
+ line_strip.header.frame_id = frame
+
+ # Set the size and color
+ line_strip.scale.x = 0.1
+ line_strip.scale.y = 0.1
+ line_strip.color.a = 1.
+ line_strip.color.r = color[0]
+ line_strip.color.g = color[1]
+ line_strip.color.g = color[2]
+
+ # Fill the line with the desired values
+ for xi, yi in zip(x, y):
+ p = Point()
+ p.x = xi
+ p.y = yi
+ line_strip.points.append(p)
+
+ # Publish the line
+ publisher.publish(line_strip)
\ No newline at end of file
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