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 main():
"""
Initializes the parameters from launch file.
Creates publishers for visualization.
Starts the instances of ConstraintsInterface, BrakeControl, Geofence and StateSubscriber.
Waits for center point and radius to be given in rviz.
Creates geofence, can be seen in rviz.
When operated through remote: If the car senses the emergency, it stops.
Or if the car is outside the geofence, it stops.
Otherwise, it runs with limited velocity.
"""
rospy.init_node('velocity_constraint_node')
# Gets the current speed limit from the launch file.
speed_limit_param = rospy.search_param('speed_limit')
speed_limit = rospy.get_param(speed_limit_param)
speed_limit = float(speed_limit)
# Gets the trigger sensitivity from the launch file
trigger_param = rospy.search_param('trigger_sensitivity')
trigger_sensitivity = rospy.get_param(trigger_param)
trigger_sensitivity = float(trigger_sensitivity)
car_poly_pub = rospy.Publisher("/3D_car", PolygonStamped, queue_size=1)
pose_pub = rospy.Publisher("/robot_pose", PoseStamped, queue_size=1)
# wait for the initial position of the car
rospy.loginfo('Waiting for initial pose')
rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
rospy.loginfo('Received initial pose')
vehicle_name = rospy.get_param(rospy.get_name() + '/vehicle_name')
# starts instance of control interface with speed constraints functionality
cont_intf = ConstraintsInterface(vehicle_name, speed_limit,
trigger_sensitivity).start()
# starts instance of brake control interface.
brake_controller = BrakeControl(vehicle_name)
# start geofence
geofence = Geofence().start()
# starting car position subscriber
car_position = StateSubscriber().start()
rospy.sleep(1)
# define the geofence
geofence.set_center()
geofence.set_radius()
# publish the geofence to rviz
geofence.define_marker()
geofence.publish_marker()
rate = rospy.Rate(30)
# sends commands from controller but with constrained speed.
# if there is an emergency the car brakes.
while not rospy.is_shutdown():
if brake_controller.is_emergency:
brake_controller.brake_car()
print('brake')
cont_intf.integral = 0 # reset integrator in PID
if geofence.is_outside_geofence(car_position):
brake_controller.brake_car_extreme()
cont_intf.integral = 0 # reset integrator in PID
else:
error = cont_intf.control_error()
velocity = cont_intf.pid_controller(error)
cont_intf.send_control(
cont_intf.remote_steering,
velocity,
0, # zero brake force
cont_intf.remote_transmission)
publish_3Dcar(car_poly_pub, pose_pub, car_position.x, car_position.y,
car_position.yaw)
rate.sleep()
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()