def main():
rospy.init_node('SVEA_sim_purepursuit')
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*init_state, dt=dt)
ctrl_interface = ControlInterface(vehicle_name).start()
rospy.sleep(0.5)
# start background simulation thread
simulator = SimSVEA(vehicle_name, simple_bicycle_model, dt)
simulator.start()
rospy.sleep(0.5)
# log data
x = []
y = []
yaw = []
v = []
t = []
# pure pursuit variables
lastIndex = len(cx) - 1
target_ind = pure_pursuit.calc_target_index(simple_bicycle_model, cx, cy)
# simualtion + animation loop
time = 0.0
while lastIndex > target_ind and not rospy.is_shutdown():
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(simple_bicycle_model, cx, cy, target_ind)
ctrl_interface.send_control(steering, target_speed)
# log data
x.append(simple_bicycle_model.x)
y.append(simple_bicycle_model.y)
yaw.append(simple_bicycle_model.yaw)
v.append(simple_bicycle_model.v)
t.append(time)
# update for animation
if show_animation:
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
else:
rospy.loginfo_throttle(1.5, simple_bicycle_model)
time += dt
if show_animation:
plt.close()
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
plt.show()
else:
# just show resulting plot if not animating
to_plot = (simple_bicycle_model, x, y)
plot_trajectory(*to_plot)
plt.show()
def track_path(self):
x = self.x
y = self.y
yaw = self.yaw
v = self.v
sp = self.spline
#s = a series of steps of 1 from 0 to the end of the spline
#Each step is 1 cm
s = np.arange(0, sp.s[-1], 1)
#Creates x, y, yaw, and curvature arrays from spline
cx, cy, cyaw, ck = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
cx.append(ix)
cy.append(iy)
cyaw.append(sp.calc_yaw(i_s))
ck.append(sp.calc_curvature(i_s))
#Determines initial state of tracker
state = tracker.State(x, y, yaw, v)
#Finds end of the spline
lastIndex = len(cx) - 1
#Calculates the first target index of the spline
target_ind = tracker.calc_target_index(state, cx, cy)
while lastIndex > target_ind:
#self.target_velocity=self.determine_velocity()
#Determines new speed of the simulation based on previous speed and target speed
ai = tracker.PIDControl(self.target_velocity, state.v)
#Delta and target index
di, target_ind = tracker.pure_pursuit_control(
state, cx, cy, target_ind)
state, yaw_change = tracker.update(state, ai, di)
self.drive_path(yaw_change)
#Will eventually be change bc these will be determined by actual position insted of simulated ones
self.x = cx[lastIndex]
self.y = cy[lastIndex]
self.yaw = state.yaw
self.v = state.v
if (not self.object_count >= 3):
self.check_beam()
if (time.time() - self.start_time > 150):
self.end_time = True
def run(self):
""" Main loop. Pure pursuit on the most recently received path. """
rospy.loginfo("Local planner waiting for odometry")
while self.odom is None and not rospy.is_shutdown():
rospy.sleep(.1)
rospy.loginfo("odometry received")
rate = rospy.Rate(20)
# This is the main loop.
while not rospy.is_shutdown():
self.path_lock.acquire()
twist = Twist()
if self.path is None: # Bail if there is no path
self.vel_pub.publish(twist)
self.path_lock.release()
rate.sleep()
continue
state = self.get_pursuit_state()
ai = pure_pursuit.PIDControl(self.target_speed, state.v)
di, self.path_index = pure_pursuit_control(state, self.path_xs,
self.path_ys,
self.path_index)
behind = False
target = util.pose_transform(self.tf_buffer,
self.path.poses[self.path_index],
'base_link')
if target.pose.position.x < 0: # behind the robot!
behind = True
goal = self.path.poses[-1]
dist = np.sqrt((state.x - goal.pose.position.x)**2 +
(state.y - goal.pose.position.y)**2)
if dist < self.goal_threshold:
twist.linear.x = 0
# Rotate to the angle of the final pose...
goal_in_base = util.pose_transform(self.tf_buffer, goal,
'base_link')
angle_error = util.yaw_from_pose(goal_in_base.pose)
twist.angular.z = np.clip(angle_error * 3, -1, 1)
elif behind or self.blocked:
twist.linear.x = 0
twist.angular.z = np.sign(di) * .5
else:
twist.linear.x = ai
wheel_base = self.wheel_base
omega = di * state.v / wheel_base
twist.angular.z = omega # very approximate
marker = make_sphere_marker(self.path_xs[self.path_index],
self.path_ys[self.path_index], 0.0,
self.path.header)
self.target_marker_pub.publish(marker)
self.vel_pub.publish(twist)
self.path_lock.release()
rate.sleep()
def main():
"""
Initializes parameters for doing donuts.
Starts instance of ConstraintsInterface and BrakeControl.
Creates publishers for visualization.
Starts instance of Geofence.
Then waits for a pose estimate for the car and geofence/donut center point to be given from rviz.
Smooth circular or donut path is created.
Car will start to follow path after start signal is given through /initialpose topic from rviz.
As soon as the car completes a lap, a new circular path with a shift in starting point is created.
Pure pursuit control is used for path tracking, PID is used for velocity control.
Geofence, circular track path and car's tracking path are visualized in rviz.
"""
rospy.init_node('donuts_node')
speed_limit, path_radius, geofence_radius, gear, target_laps = init()
# vehicle name
# initialize constraints interface
cont_intf = ConstraintsInterface(vehicle_name, speed_limit).start()
cont_intf.k_d = 100
# starts instance of brake control interface.
brake_controller = BrakeControl(vehicle_name)
rospy.sleep(2)
# start publishers
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
rospy.sleep(0.2)
# start geofence
geofence = Geofence().start()
# starting car position subscriber
car_position = StateSubscriber().start()
rospy.loginfo('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
rospy.loginfo('Received initial pose')
rospy.sleep(1)
# define the geofence
geofence.set_center_radius(geofence_radius)
# create circular path
cx, cy, cyaw = smooth_donut_path(geofence.center, path_radius)
publish_path(path_plan_pub, cx, cy)
# publish the geofence to rviz
geofence.define_marker()
geofence.publish_marker()
# wait for the initial position of the car
rospy.loginfo('Waiting for start signal')
rospy.wait_for_message('/clicked_point', PointStamped)
rospy.loginfo('Received start signal')
while not car_position.x:
print('waiting for initial car position')
# initialize pure pursuit variables
lastIndex = len(cx) - 1
# target_ind = pure_pursuit.calc_target_index(car_position, cx, cy)
target_ind = pure_pursuit.calc_target_index(car_position, cx, cy)
x = []
y = []
yaw = []
# donut loop
steering = 0.0
rate = rospy.Rate(30)
laps = 0
while laps < target_laps and not rospy.is_shutdown():
cx, cy, cyaw = smooth_donut_path(geofence.center, path_radius,
laps * math.pi / 3)
lastIndex = len(cx) - 1
target_ind = 0
while lastIndex > target_ind and not rospy.is_shutdown():
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_position, cx, cy, target_ind)
if geofence.is_outside_geofence(car_position):
brake_controller.brake_car(steering)
cont_intf.integral = 0 # reset integrator in PID
else:
cont_intf.send_constrained_control(steering, 0, gear, 1, 1)
# publish stuff for rviz.
x.append(car_position.x)
y.append(car_position.y)
yaw.append(car_position.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_position.x,
car_position.y, car_position.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
laps = laps + 1
rospy.loginfo('finished lap')
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
def train(train_epi, v, rc, it, blender):
global max_reward
history = {'episode': [], 'Episode_reward': []}
# target course
cx, cy, cv, cyaw = target_course(v, rc, it)
T = 3500 / v # max simulation time 50
total_episode = 20
sim_episode = []
sim_average_reward = []
sim_a_RL_percentage = []
sim_d_RL_percentage = []
for i in range(total_episode):
# initial state
state = State(x=-0.0, y=0.0, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
sim_t = []
sim_x = []
sim_y = []
sim_yaw = []
sim_v = []
sim_a_PID = []
sim_d_PID = []
sim_d_pure_pursuit = []
sim_a_final = []
sim_d_final = []
sim_a_RL = []
sim_d_RL = []
sim_a_RL_correction = []
sim_d_RL_correction = []
sim_a_RL_or_not = []
sim_d_RL_or_not = []
sim_reward = []
sim_target_dis = []
sim_target_angle = []
d_PID = 0
a_final = 0
d_final = 0
target_speed = cv[0]
# observation = np.array([target_dis, target_angle, target_speed, state.v, a_final, d_final])
target_ind = calc_target_index(state, cx, cy, 1)
target_dis, target_angle = target_dis_dir(state.x, state.y, state.yaw,
cx[target_ind],
cy[target_ind])
# observation = np.array([target_dis, target_angle, target_speed, state.v, a_final, d_final])
# observation = np.array([target_angle])
observation = np.array([target_angle, target_speed])
# print(observation)
states, actions, rewards = [], [], []
episode_reward = 0
j = 0
while T >= time and lastIndex > target_ind:
# print('observation',observation)
target_speed = cv[target_ind]
# a_PID = a_PIDControl(target_speed, state.vx)
a_PID = a_PIDControl(target_speed, state.v)
# print(target_speed, state.v)
# if abs(a_PID) > 5:
# a_PID = np.sign(a_PID)*5
# a_PID = PIDControl(target_speed, state.v)
d_pure_pursuit, target_ind = pure_pursuit_control(
state, cx, cy, target_ind, 0)
# print(d_pure_pursuit)
d_PID = d_PIDControl(target_angle, d_PID)
# print(target_angle)
action_RL = model.choose_action(observation)
# print(observation, action_RL)
a_RL = 0 # acceleration
d_RL = action_RL[0] # delta_steer
if math.isnan(d_RL):
# print('observation',observation)
# print('d_RL is none')
d_RL = 0
j = 999999999
break
# continue
# print('d_RL',d_RL)
# Control command mechanism selection
# switch
# a_final, d_final, a_RL_or_not, d_RL_or_not = control_switch(a_PID, d_pure_pursuit, a_RL, d_RL)
# adjust
# a_final, d_final = control_blender(a_PID, d_pure_pursuit, a_RL, d_RL)
# pure ppo
# a_final, d_final = a_RL, d_RL
# pure conventional
# a_final, d_final = a_PID, d_pure_pursuit
# a_final, d_final, a_RL_correction, d_RL_correction = control_blender(a_PID, d_PID, a_RL, d_RL)
a_final, d_final, a_RL_correction, d_RL_correction = control_blender(
a_PID, d_pure_pursuit, a_RL, d_RL, blender)
# state = update(state, a_final, d_final)
# state = update_cv(state, target_speed, d_final)
state = dynamic_update_cv(state, target_speed, d_final)
# state = update(state, a_RL, d_RL)
# state = kinematic_update(state, a_final, d_final)
# state = dynamic_update(state, a_PID, d_pure_pursuit)
# state = dynamic_update(state, a_final, d_final)
target_dis, target_angle = target_dis_dir(state.x, state.y,
state.yaw,
cx[target_ind],
cy[target_ind])
# print('target_dis, target_angle',target_dis, target_angle)
# next_observation = np.array([target_dis/50, target_angle, target_speed/50, state.v/50, a_final/10, d_final/20])
next_observation = np.array([target_angle, target_speed])
reward = reward_function(state, cx, cy, a_PID, d_pure_pursuit,
a_RL, d_RL, target_dis, target_angle,
target_speed, state.v)
episode_reward += reward
states.append(observation)
actions.append(action_RL)
rewards.append(reward)
observation = next_observation
# print(observation)
sim_t.append(time)
sim_x.append(state.x)
sim_y.append(state.y)
sim_yaw.append(state.yaw)
sim_v.append(state.v)
sim_a_PID.append(a_PID)
sim_d_PID.append(d_PID)
sim_d_pure_pursuit.append(d_pure_pursuit)
sim_a_RL.append(a_RL)
sim_d_RL.append(d_RL)
sim_a_RL_correction.append(a_RL_correction)
sim_d_RL_correction.append(d_RL_correction)
sim_a_final.append(a_final)
sim_d_final.append(d_final)
sim_target_dis.append(target_dis)
sim_target_angle.append(target_angle)
# sim_a_RL_or_not.append(a_RL_or_not)
# sim_d_RL_or_not.append(d_RL_or_not)
sim_reward.append(reward)
# show_step_animation(cx, cy, sim_x, sim_y, sim_yaw, state.x, state.y, state.yaw, state.v, target_ind)
# w = model.get_weights(i)
if (j + 1) % model.batch == 0:
states = np.array(states)
actions = np.array(actions)
rewards = np.array(rewards)
d_reward = model.discount_reward(states, rewards,
next_observation)
# d_reward = [reward]
# print('states',states,'actions',actions,'rewards',rewards)
# print(d_reward)
model.update(states, actions, d_reward, j)
# w = model.get_weights(i)
states, actions, rewards = [], [], []
j += 1
time = time + dt
# print(len(sim_d_RL_or_not))
average_reward = episode_reward / j
sim_episode.append(i)
sim_average_reward.append(average_reward)
# sim_a_RL_percentage.append(np.sum(sim_a_RL_or_not)/len(sim_a_RL_or_not)*100)
# sim_d_RL_percentage.append(np.sum(sim_d_RL_or_not)/len(sim_d_RL_or_not)*100)
# history['episode'].append(i)
# history['Episode_reward'].append(episode_reward)
# print('Episode: {} | Episode reward: {:.2f}'.format(i, average_reward))
# model.save_history(history, 'ppo2.csv')
# w = model.get_weights(i)
# print(w)
if train_epi == 0:
if i == 0:
max_reward = average_reward
print('max_reward:', round(max_reward, 2))
save_io = 0
# save_plot(i,average_reward)
save_sim(i,average_reward,cx, cy, cv, sim_t, sim_x, sim_y, sim_yaw, sim_v, \
sim_a_PID, sim_d_PID, sim_d_pure_pursuit, sim_a_RL, sim_d_RL, sim_a_final, sim_d_final, \
sim_reward, sim_episode, sim_average_reward, \
sim_a_RL_correction, sim_d_RL_correction, sim_target_dis, sim_target_angle)
show_episode_plot(cx, cy, cv, sim_t, sim_x, sim_y, sim_yaw, sim_v, \
sim_a_PID, sim_d_PID, sim_d_pure_pursuit, sim_a_RL, sim_d_RL, sim_a_final, sim_d_final, \
sim_reward, sim_episode, sim_average_reward, sim_a_RL_or_not, sim_d_RL_or_not, sim_a_RL_percentage, sim_d_RL_percentage, \
sim_a_RL_correction, sim_d_RL_correction, sim_target_dis, sim_target_angle)
plot_action(model, save_io, i, average_reward, target_speed)
strategy_3d_map(model)
if average_reward > max_reward:
save_io = 1
max_reward = average_reward
print('Better! Save Model! max_reward:', round(max_reward, 2))
model.save_learning()
show_episode_plot(cx, cy, cv, sim_t, sim_x, sim_y, sim_yaw, sim_v, \
sim_a_PID, sim_d_PID, sim_d_pure_pursuit, sim_a_RL, sim_d_RL, sim_a_final, sim_d_final, \
sim_reward, sim_episode, sim_average_reward, sim_a_RL_or_not, sim_d_RL_or_not, sim_a_RL_percentage, sim_d_RL_percentage, \
sim_a_RL_correction, sim_d_RL_correction, sim_target_dis, sim_target_angle)
plot_action(model, save_io, i, average_reward, target_speed)
strategy_3d_map(model)
else:
if average_reward > max_reward:
save_io = 1
max_reward = average_reward
print('Better! Save Model! max_reward:', round(max_reward, 2))
# print('Better! Save Model!')
model.save_learning()
# save_plot(i,average_reward)
save_sim(i,average_reward,cx, cy, cv, sim_t, sim_x, sim_y, sim_yaw, sim_v, \
sim_a_PID, sim_d_PID, sim_d_pure_pursuit, sim_a_RL, sim_d_RL, sim_a_final, sim_d_final, \
sim_reward, sim_episode, sim_average_reward, \
sim_a_RL_correction, sim_d_RL_correction, sim_target_dis, sim_target_angle)
show_episode_plot(cx, cy, cv, sim_t, sim_x, sim_y, sim_yaw, sim_v, \
sim_a_PID, sim_d_PID, sim_d_pure_pursuit, sim_a_RL, sim_d_RL, sim_a_final, sim_d_final, \
sim_reward, sim_episode, sim_average_reward, sim_a_RL_or_not, sim_d_RL_or_not, sim_a_RL_percentage, sim_d_RL_percentage, \
sim_a_RL_correction, sim_d_RL_correction, sim_target_dis, sim_target_angle)
plot_action(model, save_io, i, average_reward, target_speed)
strategy_3d_map(model)
else:
save_io = 0
def main():
"""
Initializes parameters for pure pursuit.
Starts instance of ConstraintsInterface and BrakeControl.
Starts instance of StateSubscriber.
Creates publishers for visualization.
Then waits for a pose estimate for the car to be given.
Car will start to follow path after start signal is given,
which is done by publishing to the /clicked_point topic.
"""
global short_look_ahead, long_look_ahead
rospy.init_node('floor2_pure_pursuit')
vehicle_name = "SVEA3"
target_speed, cx, cy, gear, long_look_ahead, short_look_ahead = init()
final_point = (-8.45582103729, -5.04866838455)
cx.append(final_point[0])
cy.append(final_point[1])
cont_intf = ConstraintsInterface(vehicle_name, target_speed).start()
cont_intf.k_d = 100
while not cont_intf.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
# start emergency brake node
brake_controller = BrakeControl(vehicle_name)
# start car position and velocity subscriber
car_state = StateSubscriber().start()
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
rospy.sleep(3)
print('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
print('Received initial pose')
publish_path(path_plan_pub, cx, cy)
print('Waiting for start signal')
rospy.wait_for_message('/clicked_point', PointStamped)
print('Received start signal')
while not car_state.x:
print('waiting for initial car position')
# PURE PURSUIT PARAMS ########################################################
pure_pursuit.k = 0.1 # look forward gain
pure_pursuit.Lfc = long_look_ahead # look-ahead distance
pure_pursuit.L = 0.324 # [m] wheel base of vehicle
###############################################################################
lastIndex = len(cx) - 2
target_ind = pure_pursuit.calc_target_index(car_state, cx, cy)
x = []
y = []
yaw = []
# pure pursuit loop
# follow path until second to last index.
steering = 0.0
rate = rospy.Rate(30)
cont_intf.send_control(steering, 0, 0, gear) # send low gear
while lastIndex > target_ind and not rospy.is_shutdown():
pure_pursuit.Lfc = calc_look_ahead(car_state)
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_state, cx, cy, target_ind)
if not brake_controller.is_emergency:
cont_intf.send_constrained_control(
steering,
0, # zero brake force
gear) # send low gear
else:
brake_controller.brake_car(steering)
# publish car, path and target index for rviz
x.append(car_state.x)
y.append(car_state.y)
yaw.append(car_state.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_state.x, car_state.y,
car_state.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
# create a straight path for points inbetween the last two points
# points to project goal onto
p1 = (-2.33859300613, 3.73132610321)
p2 = (-2.86509466171, 3.14682602882)
line = (p2[0] - p1[0], p2[1] - p1[1])
proj_p2 = np.dot(p2, line)
cx = cx[:-1]
cy = cy[:-1]
new_x = np.linspace(cx[-1], final_point[0], 50)
new_y = np.linspace(cy[-1], final_point[1], 50)
cx.extend(new_x)
cy.extend(new_y)
# continue following final point until goal is reached.
while not rospy.is_shutdown():
steering, target_ind = \
pure_pursuit.pure_pursuit_control(car_state, cx, cy, target_ind)
# calculate car projection on line
proj_car = np.dot((car_state.x, car_state.y), line)
if brake_controller.is_emergency:
brake_controller.brake_car(steering)
elif proj_car < proj_p2:
cont_intf.send_constrained_control(steering, 0, gear)
else:
# brake if goal is reached.
brake_controller.brake_car(steering)
rospy.sleep(0.1)
break
# update visualizations
x.append(car_state.x)
y.append(car_state.y)
yaw.append(car_state.yaw)
publish_3Dcar(car_poly_pub, pose_pub, car_state.x, car_state.y,
car_state.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
rate.sleep()
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
while brake_controller.is_emergency:
brake_controller.brake_car(steering)
rospy.spin()
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)
if (previous_state not in [3,5,6,7]):
previous_state = state
AUTO_enter_timer.reset()
if rp.use_AUTO:
if not AUTO_enter_timer.check():
print "waiting before entering AUTO mode..."
steer_cmd_pub.publish(std_msgs.msg.Int32(49))
motor_cmd_pub.publish(std_msgs.msg.Int32(49))
time.sleep(0.1)
continue
else:
state = State(gps2x(GPS2_long), gps2y(GPS2_lat), gps2yaw(GPS2_angle), gps2v(GPS2_speed))
AUTO_motor = pid_control(target_speed, gps2v(GPS2_speed))
AUTO_steer = pure_pursuit_control(state, cx, cy, target_ind)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
if AUTO_motor > rp.motor_freeze_threshold \
and np.array(encoder_list[0:3]).mean() > 1 \
and np.array(encoder_list[-3:]).mean()<0.2 \
and state_transition_time_s > 1:
freeze = True
if lastIndex >= target_ind:
print(Goal!)
freeze = True
# avoid experiment too long
if finish_test_timer.check():
freeze = True
def main():
"""
Initializes the parameters from the launch file.
Creates circular path.
Starts the instances of SimpleBicycleState, ControlInterface and SimSVEA.
Creates publishers for visualization.
Pure pursuit is used for path tracking. PID is used for limiting velocity.
"""
rospy.init_node('floor2_example')
start_pt, use_rviz, use_matplotlib, cx, cy = init()
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*start_pt, dt=dt)
ctrl_interface = ControlInterface(vehicle_name).start()
rospy.sleep(0.2)
# start background simulation thread
simulator = SimSVEA(vehicle_name, simple_bicycle_model, dt)
simulator.start()
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
path_plan_pub = rospy.Publisher("/path_plan", Path, queue_size=1)
past_path_pub = rospy.Publisher("/past_path", Path, queue_size=1)
target_pub = rospy.Publisher("/target", PointStamped, queue_size=1)
rospy.sleep(0.2)
# log data
x = []
y = []
yaw = []
v = []
t = []
# initialize pure pursuit variables
lastIndex = len(cx) - 1
target_ind = pure_pursuit.calc_target_index(simple_bicycle_model, cx, cy)
print(target_ind)
# simualtion + animation loop
time = 0.0
steering = 0.0
while lastIndex > target_ind and not rospy.is_shutdown():
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(simple_bicycle_model, cx, cy, target_ind)
ctrl_interface.send_control(steering, target_speed)
# log data; mostly used for visualization
x.append(simple_bicycle_model.x)
y.append(simple_bicycle_model.y)
yaw.append(simple_bicycle_model.yaw)
v.append(simple_bicycle_model.v)
t.append(time)
# update visualizations
if use_rviz:
publish_3Dcar(car_poly_pub, pose_pub, simple_bicycle_model.x,
simple_bicycle_model.y, simple_bicycle_model.yaw)
publish_path(path_plan_pub, cx, cy)
publish_path(past_path_pub, x, y, yaw)
publish_target(target_pub, cx[target_ind], cy[target_ind])
if use_matplotlib:
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
if not (use_rviz or use_matplotlib):
rospy.loginfo_throttle(1.5, simple_bicycle_model)
time += dt
if not rospy.is_shutdown():
rospy.loginfo("Trajectory finished.")
if use_matplotlib:
plt.close()
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
plt.show()
def track_path(self):
x = self.x
y = self.y
yaw = self.yaw
v = self.v
sp = self.spline
#s = a series of steps of 1 from 0 to the end of the spline
#Each step is 1 cm
s = np.arange(0, sp.s[-1], 1)
#Creates x, y, yaw, and curvature arrays from spline
cx, cy, cyaw, ck = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
cx.append(ix)
cy.append(iy)
cyaw.append(sp.calc_yaw(i_s) * 180 / (math.pi))
ck.append(sp.calc_curvature(i_s))
#Determines initial state of tracker
state = tracker.State(x, y, yaw, v)
#Finds end of the spline
lastIndex = len(cx) - 1
#Calculates the first target index of the spline
target_ind = tracker.calc_target_index(state, cx, cy, 0)
while lastIndex > target_ind:
#self.target_velocity=self.determine_velocity()
#Determines new speed of the simulation based on previous speed and target speed
#ai = tracker.PIDControl(self.target_velocity, state.v)
#Delta and target index
#di, target_ind = tracker.pure_pursuit_control(state, cx, cy, target_ind)
target_ind = tracker.pure_pursuit_control(state, cx, cy,
target_ind)
#state, yaw_change = tracker.update(state, ai, di)
yaw_change = cyaw[target_ind] - pose[2]
print("Yaw Index: " + str(target_ind))
print("Next: " + str(cyaw[target_ind]) + " Current: " +
str(pose[2]))
print("So change is " + str(yaw_change))
print(
"--------------------------------------------------------------------------------"
)
state.x = pose[0]
state.y = pose[1]
state.yaw = pose[2]
state.v = self.v
#print(str(yaw_change))
self.drive_path(yaw_change)
time.sleep(0.15) #no greater than 0.15
v = self.calculate_velocity(self.x, pose[0], self.y, pose[1])
if (v == 0):
v = self.v
self.v = v
self.x = pose[0]
self.y = pose[1]
self.yaw = pose[2]
#print("V: "+str(self.v))
#with open("data.txt","a") as file:
# file.write("X: " + str(self.x) + ", Y: " + str(self.y) + ", Yaw: " + str(self.yaw) + ", V: " + str(self.v) + "\n")
if (not self.object_count >= 3):
self.check_beam()
if (time.time() - self.start_time > 20):
self.end_time = True
def create_path(self):
# Increase the "resolution" of the path with 15 intermediate points
local_points_plus = self.local_points # geometry_util.add_intermediate_points(self.local_points, 15.0)
ax = []
ay = []
# Create a Path object for displaying the raw path (no spline) in RViz
display_points = Path()
display_points.header = Header()
display_points.header.frame_id = '/map'
# Set the beginning of the navigation the first point
last_index = 0
target_ind = 0
# Creates a list of the x's and y's to be used when calculating the spline
for p in local_points_plus:
display_points.poses.append(create_pose_stamped(p))
ax.append(p.x)
ay.append(p.y)
self.points_pub.publish(display_points)
# If the path doesn't have any successive points to navigate through, don't try
if len(ax) > 2:
# Create a cubic spline from the raw path
cx, cy, cyaw, ck, cs = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
# Create Path object which displays the cubic spline in RViz
path = Path()
path.header = Header()
path.header.frame_id = '/map'
# Add cubic spline points to path
for i in range(0, len(cx)):
curve_point = Point()
curve_point.x = cx[i]
curve_point.y = cy[i]
path.poses.append(create_pose_stamped(curve_point))
self.path_pub.publish(path)
# Set the current state of the cart to navigating
self.current_state = VehicleState()
self.current_state.is_navigating = True
self.vehicle_state_pub.publish(self.current_state)
target_speed = self.global_speed
# initial state
pose = self.global_pose
twist = self.global_twist
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
initial_v = twist.linear.x
#TODO state has to be where we start
state = State(x=pose.position.x,
y=pose.position.y,
yaw=angles[2],
v=initial_v)
# last_index represents the last point in the cubic spline, the destination
last_index = 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, 0)
# Publish the ETA to the destination before we get started
self.calc_eta(None)
# Continue to loop while we have not hit the target destination, and the path is still valid
while last_index > target_ind and self.path_valid and not rospy.is_shutdown(
):
target_speed = self.global_speed
ai = target_speed #pure_pursuit.PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit.pure_pursuit_control(
state, cx, cy, target_ind)
#publish our desired position
mkr = create_marker(cx[target_ind], cy[target_ind], '/map')
self.target_pub.publish(mkr)
# Arrow that represents steering angle
arrow = 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
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)
state = self.update(state, ai, di)
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
else:
self.path_valid = False
rospy.logwarn("It appears the cart is already at the destination")
#Check if we've reached the destination, if so we should change the cart state to finished
rospy.loginfo("Done navigating")
self.current_state = VehicleState()
self.current_state.is_navigating = False
self.current_state.reached_destination = True
notify_server = String()
# Let operator know why current path has stopped
if self.path_valid:
rospy.loginfo(
"Reached Destination succesfully without interruption")
self.arrived_pub.publish(notify_server)
else:
rospy.loginfo(
"Already at destination, or there may be no path to get to the destination or navigation was interrupted."
)
# Update the internal state of the vehicle
self.vehicle_state_pub.publish(self.current_state)
msg = VelAngle()
msg.vel = 0
msg.angle = 0
msg.vel_curr = 0
self.motion_pub.publish(msg)
def main():
rospy.init_node('SVEA_sim_multi')
# initialize simulated model and control interface
simple_bicycle_model0 = SimpleBicycleState(*init_state0, dt=dt)
ctrl_interface0 = ControlInterface(vehicle_name0).start()
simple_bicycle_model1 = SimpleBicycleState(*init_state1, dt=dt)
ctrl_interface1 = ControlInterface(vehicle_name1).start()
rospy.sleep(0.5)
# start background simulation thread
sim_car0 = SimSVEA(vehicle_name0, simple_bicycle_model0, dt)
sim_car1 = SimSVEA(vehicle_name1, simple_bicycle_model1, dt)
sim_car0.start()
sim_car1.start()
rospy.sleep(0.5)
# log data
log0 = {"x": [], "y": [], "yaw": [], "v": [], "t": []}
log1 = {"x": [], "y": [], "yaw": [], "v": [], "t": []}
# pure pursuit variables
lastIndex = len(cx) - 1
target_ind0 = pure_pursuit.calc_target_index(simple_bicycle_model0, cx, cy)
target_ind1 = pure_pursuit.calc_target_index(simple_bicycle_model1, cx, cy)
# simualtion + animation loop
time = 0.0
while lastIndex > target_ind0 and not rospy.is_shutdown():
# compute control input via pure pursuit
steering0, target_ind0 = \
pure_pursuit.pure_pursuit_control(simple_bicycle_model0, cx, cy, target_ind0)
ctrl_interface0.send_control(steering0, target_speed)
steering1, target_ind1 = \
pure_pursuit.pure_pursuit_control(simple_bicycle_model1, cx, cy, target_ind1)
ctrl_interface1.send_control(steering1, target_speed)
# log data
log0 = log_data(log0, simple_bicycle_model0, time)
log1 = log_data(log1, simple_bicycle_model1, time)
# update for animation
if show_animation:
to_plot0 = (simple_bicycle_model0,
log0["x"], log0["y"],
steering0, vehicle_color0,
target_ind0)
to_plot1 = (simple_bicycle_model1,
log1["x"], log1["y"],
steering1, vehicle_color1,
target_ind1)
animate_multi([to_plot0, to_plot1])
else:
rospy.loginfo_throttle(1.5, simple_bicycle_model0)
rospy.loginfo_throttle(1.5, simple_bicycle_model1)
time += dt
if show_animation:
plt.close()
to_plot0 = (simple_bicycle_model0,
log0["x"], log0["y"],
steering0, vehicle_color0,
target_ind0)
to_plot1 = (simple_bicycle_model1,
log1["x"], log1["y"],
steering1, vehicle_color1,
target_ind1)
animate_multi([to_plot0, to_plot1])
plt.show()
else:
# just show resulting plot if not animating
to_plot0 = (simple_bicycle_model0,
log0["x"], log0["y"],
vehicle_color0)
to_plot1 = (simple_bicycle_model1,
log1["x"], log1["y"],
vehicle_color1)
plot_trajectory(*to_plot0)
plot_trajectory(*to_plot1)
plt.show()
