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 main():
rospy.init_node('SVEA_sim_keyboard')
# 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 = []
# hook callback for updating keyboard commands
rospy.Subscriber("/key_vel", Twist, update_key_teleop)
# simualtion + animation loop
r = rospy.Rate(30) #SVEA cars operate on 30Hz!
while not rospy.is_shutdown():
# pass keyboard commands to ctrl interface
ctrl_interface.send_control(steering, velocity)
# 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)
# update for animation
if show_animation:
plt.cla()
plt.plot(x, y, "-b", label="trajectory")
plot_car(simple_bicycle_model.x, simple_bicycle_model.y,
simple_bicycle_model.yaw, steering)
plt.axis("equal")
plt.grid(True)
plt.title("Heading[deg]: " +
str(round(math.degrees(simple_bicycle_model.yaw), 4)) +
" | Speed[m/s]:" + str(round(simple_bicycle_model.v, 4)))
plt.pause(0.001)
# sleep so loop runs at 30Hz
r.sleep()
def main():
"""
Starts instance of ControlInterface and waits for initialization.
Sends move forward, move backward, turn left and turn right commands to the car for 3 seconds each.
"""
rospy.init_node('lli_testing')
vehicle_name = 'SVEA3'
ctrl_interface = ControlInterface(vehicle_name).start()
rate = 10
r = rospy.Rate(rate)
while not ctrl_interface.is_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
rospy.loginfo(ctrl_interface)
run_time = 3
"""
rospy.loginfo("Engaging transmission for 3 seconds")
i = 0
while not rospy.is_shutdown() and i < rate*run_time:
ctrl_interface.send_control(0, 0, transmission=0)
r.sleep()
i += 1
"""
rospy.loginfo("Going forward for 3 seconds")
i = 0
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(0, 0.3, transmission=0)
r.sleep()
i += 1
rospy.loginfo("Going backwards for 3 seconds")
i = 0
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(0, -0.3, transmission=0)
r.sleep()
i += 1
i = 0
rospy.loginfo("Braking for 3 seconds")
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(brake_force=20, transmission=0)
r.sleep()
i += 1
i = 0
rospy.loginfo("Going backwards for 3 seconds")
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(0, -0.3, transmission=0)
r.sleep()
i += 1
i = 0
rospy.loginfo("Turning left for 3 seconds")
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(10, 0, transmission=0)
r.sleep()
i += 1
i = 0
rospy.loginfo("Turning right for 3 seconds")
while not rospy.is_shutdown() and i < rate * run_time:
ctrl_interface.send_control(-10, 0, transmission=0)
r.sleep()
i += 1
rospy.loginfo("Going idle")
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()
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()