def waypoints_callback(self, msg):
#replace the waypoints array with new data
xyz_points = []
for gps_point in msg.waypoints:
point = PointStamped()
point.header = Header()
point.header.frame_id = '/odom'
point.point = gps_util.get_point(gps_point)
xyz_points.append(point)
self.xyz_waypoint_pub.publish(point)
def get_point_at_index(self, index):
if not self.waypoints or index >= len(self.waypoints):
return None
goal = self.waypoints[index]
xyz_goal = gps_util.get_point(goal.latitude, goal.longitude)
goal_point = Point()
goal_point.x = xyz_goal.x
goal_point.y = xyz_goal.y
goal_point.z = 0 #elevation is always 0
return goal_point
def waypoints_callback(self, msg):
"""
Waypoints callback does way too much right now. All of the path stuff should be handled in a helper method
"""
google_points = []
#Reads each point in the waypoint topic into google_points
for gps_point in msg.waypoints:
point = gps_util.get_point(gps_point)
google_points.append(point)
print len(google_points)
#Adds more points between the google points
google_points_plus = gps_util.add_intermediate_points(
google_points, 15.0)
print len(google_points_plus)
ax = []
ay = []
extra_points = Path()
extra_points.header = Header()
extra_points.header.frame_id = '/map'
#Puts the x's and y's
for p in google_points_plus:
extra_points.poses.append(self.create_poseStamped(p))
ax.append(p.x)
ay.append(p.y)
self.points_pub.publish(extra_points)
#calculate the spline
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
path = Path()
path.header = Header()
path.header.frame_id = '/map'
for i in range(0, len(cx)):
curve_point = Point()
curve_point.x = cx[i]
curve_point.y = cy[i]
path.poses.append(self.create_poseStamped(curve_point))
self.path_pub.publish(path)
#================================================ pure persuit copy/pase ===============================================
k = 0.1 # look forward gain
Lfc = 3.5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
target_speed = 10.0 / 3.6 # [m/s]
T = 100.0 # max simulation time
# initial state
pose = self.odom.pose.pose
twist = self.odom.twist.twist
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
initial_v = math.sqrt(twist.linear.x**2 + twist.linear.y**2)
state = State(x=pose.position.x,
y=pose.position.y,
yaw=angles[2],
v=initial_v) #TODO this has to be where we start
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = pure_pursuit.calc_target_index(state, cx, cy)
while lastIndex > target_ind:
ai = pure_pursuit.PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit.pure_pursuit_control(
state, cx, cy, target_ind)
#publish where we want to be
mkr = self.create_marker(cx[target_ind], cy[target_ind], '/map')
self.target_pub.publish(mkr)
#publish an arrow with our twist
arrow = self.create_marker(0, 0, '/base_link')
arrow.type = 0 #arrow
arrow.scale.x = 2.0
arrow.scale.y = 1.0
arrow.scale.z = 1.0
arrow.color.r = 1.0
arrow.color.g = 0.0
arrow.color.b = 0.0
#TODO di might be in radians so that might be causing the error
quater = tf.quaternion_from_euler(0, 0, di)
arrow.pose.orientation.x = quater[0]
arrow.pose.orientation.y = quater[1]
arrow.pose.orientation.z = quater[2]
arrow.pose.orientation.w = quater[3]
self.target_twist_pub.publish(arrow)
#go back to pure persuit
state = self.update(state, ai, di)
#time = time + dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
# Test
assert lastIndex >= target_ind, "Cannot goal"
rospy.logerr("Done navigating")
msg = VelAngle()
msg.vel = 0
msg.angle = 0
msg.vel_curr = 0
self.motion_pub.publish(msg)
def gps_array_to_xyz_array(gps_array):
return geometry_util.add_intermediate_points(
[gps_util.get_point(gps.latitude, gps.longitude) for gps in gps_array],
1)