def visualize_everything(self):
"""
Publish car, path and target index for rviz.
"""
publish_3Dcar(self.car_poly_pub, self.pose_pub, self.car_position.x,
self.car_position.y, self.car_position.yaw)
publish_path(self.path_plan_pub, self.cx, self.cy)
publish_target(self.target_pub, self.cx[self.target_ind],
self.cy[self.target_ind])
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 new_path(self):
"""
Calculates obstacle avoiding path and publishes it for the pure pursuit node.
For the distant obstacles, the path planning is done taking shorter time.
For the near obstacles, the path planning is done taking longer time.
Publishes the obstacle markers for visualization in rviz.
:return: returning null during exception and empty path
:rtype: null
"""
try:
#1)Find index of where we are
dx = [self.car_position.x - icx for icx in self.cx]
dy = [self.car_position.y - icy for icy in self.cy]
d = [abs(sqrt(idx**2 + idy**2)) for (idx, idy) in zip(dx, dy)]
index = d.index(min(d)) # index of point nearest to car
#2)Find start index and point
d_temp = [abs(x) for x in d[index:index + 500]]
s_ind = d_temp.index(min(d_temp)) + index
start_x = self.cx[s_ind]
start_y = self.cy[s_ind]
# Help index to avoid planning from start to end through the big corridor
temp_length = len(self.cx)
end_ind = -1
if s_ind < temp_length / 2:
end_ind = int(2 * s_ind)
#3)Find target index and point
dx = [start_x - icx for icx in self.cx[s_ind:end_ind]]
dy = [start_y - icy for icy in self.cy[s_ind:end_ind]]
d_temp = [
abs(sqrt(idx**2 + idy**2) - 4) for (idx, idy) in zip(dx, dy)
]
t_ind = d_temp.index(min(d_temp)) + s_ind
target_x = self.cx[t_ind]
target_y = self.cy[t_ind]
except:
print("Index error")
return
# Publish start and target for the replanner
self.target_pub.publish(
self._init_marker(target_x, target_y, 1, [0, 0, 1]))
self.target_pub.publish(
self._init_marker(start_x, start_y, 4, [1, 1, 0]))
print("Planning running")
try:
if self.plan_longer:
# Plan longer with timeout 11
self.plan_longer = False
x, y = self.a_star.planning_longer(start_x,
start_y,
target_x,
target_y,
self.map_service.get_map(),
inflate=True)
else:
# Plann with timout 2
x, y = self.a_star.planning(start_x,
start_y,
target_x,
target_y,
self.map_service.get_map(),
inflate=True)
if len(x) < 25:
return
except:
print("Couldnt find any path")
return
if len(x) > 0:
# New Path
cx_new = self.cx[0:index]
cx_new.extend(x)
cx_new.extend(self.cx[t_ind:])
cy_new = self.cy[0:index]
cy_new.extend(y)
cy_new.extend(self.cy[t_ind:])
self.cx = cx_new
self.cy = cy_new
self.pub_marker(self.cx, self.cy)
# Create message for new path and publish
new_path = path_pure_pursuit()
new_path.cx = cx_new
new_path.cy = cy_new
publish_path(self.path_pub, cx_new, cy_new)
self.path_update.publish(new_path)
else:
rospy.loginfo("Got an empty path and moving on.")
return
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 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 main():
"""
Initializes the parameters from the launch file.
Creates path using dubins planner.
Starts the instances of SimpleBicycleState, ControlInterface and SimSVEA.
Creates publishers for visualization.
LQR is used for path tracking. PID is used for limiting velocity.
"""
rospy.init_node('floor2_sim_lqr')
# start_pt, use_rviz, use_matplotlib, cx, cy = init()
start_pt, use_rviz, use_matplotlib, cx, cy, cyaw, ck = init()
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*start_pt, dt=dt)
ctrl_interface = ControlInterface(vehicle_name).start()
while not ctrl_interface.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
# 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)
# target_ind, tmp = lqr_steer_control.calc_nearest_index(simple_bicycle_model, cx, cy, cyaw)
target_ind, tmp = lqr_speed_steer_control.calc_nearest_index(
simple_bicycle_model, cx, cy, cyaw)
speed_profile = lqr_speed_steer_control.calc_speed_profile(
cx, cy, cyaw, target_speed)
# target_ind, tmp = rear_wheel_feedback.calc_nearest_index(simple_bicycle_model, cx, cy, cyaw)
print("tar:", target_ind)
# simualtion + animation loop
time = 0.0
steering = 0.0
e, e_th = 0.0, 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)
# steering, target_ind, e, e_th = lqr_steer_control.lqr_steering_control(
# simple_bicycle_model, cx, cy, cyaw, ck, e, e_th)
steering, target_ind, e, e_th, ai = \
lqr_speed_steer_control.lqr_steering_control(simple_bicycle_model, cx, cy, cyaw, ck, e, e_th, speed_profile)
# if steering >= max_steer:
# steering = max_steer
# if steering <= - max_steer:
# steering = - max_steer
# steering, target_ind = \
# rear_wheel_feedback.rear_wheel_feedback_control(simple_bicycle_model, cx, cy, cyaw, ck, 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()
rospy.spin()