def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
# Sensors
self.odometry = odometry.Odometry()
self.tracker = factory.create_tracker(1,
sd_x=0.01,
sd_y=0.01,
sd_theta=0.01,
rate=10)
# Controllers for Odometry
self.pidTheta = pid_controller_soln.PIDController(300,
5,
50, [-10, 10],
[-180, 180],
is_angle=True)
self.pidDistance = pid_controller_soln.PIDController(1000,
0,
50, [0, 0],
[-500, 500],
is_angle=False)
# Pen Holder - raises and lowers pen
self.penholder = factory.create_pen_holder()
# Image to be drawn
self.img = lab11_image.VectorImage("lab11_img1.yaml")
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
# read file and get all paths
self.img = lab11_image.VectorImage("lab11_img1.yaml")
# init objects
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.penholder = factory.create_pen_holder()
self.tracker = Tracker(factory.create_tracker,
1,
sd_x=0.01,
sd_y=0.01,
sd_theta=0.01,
rate=10)
self.servo = factory.create_servo()
self.odometry = Odometry()
# alpha for tracker
self.alpha_x = 0.3
self.alpha_y = self.alpha_x
self.alpha_theta = 0.3
# init controllers
self.pidTheta = PIDController(200,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
self.pidDistance = PIDController(500,
0,
50, [0, 0], [-200, 200],
is_angle=False)
self.filter = ComplementaryFilter(
self.odometry, None, self.time, 0.4,
(self.alpha_x, self.alpha_y, self.alpha_theta))
# parameters
self.base_speed = 100
# constant
self.robot_marker_distance = 0.1906
# debug vars
self.debug_mode = True
self.odo = []
self.actual = []
self.xi = 0
self.yi = 0
self.init = True
self.penholder_depth = -0.025
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500, 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(1000, 0, 50, [0, 0], [-200, 200], is_angle=False)
self.tracker = factory.create_tracker(1, sd_x=0.01, sd_y=0.01, sd_theta=0.01, rate=10)
self.nodes = []
self.alpha = 0.9999
self.beta = 1 - self.alpha
# read file
self.img = lab11_image.VectorImage("lab11_img1.yaml")
self.penholder = factory.create_pen_holder()
self.offset = 0.18
"""
import argparse
import numpy as np
import matplotlib.pyplot as plt
import math
import lab11_image
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("image", help="vector image file to plot (YAML)")
args = parser.parse_args()
# read file
img = lab11_image.VectorImage(args.image)
# setup figure
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal')
# draw all bezier curves
for path in img.paths:
ts = np.linspace(0, 1.0, 100)
result = np.empty((0, 3))
for i in range(0, path.num_segments()):
for t in ts[:-2]:
s = path.eval(i, t)
result = np.vstack([result, s])
ax.plot(result[:, 0], result[:, 1], path.color)
for tie_index in tie_list_[:, 0]:
if tie_index >= len(lines):
curr_color = paths_color[tie_index - len(lines)]
else:
curr_color = lines[tie_index].color
if curr_color == prev_color_:
same_color_filter_.append(tie_index)
if len(same_color_filter_) > 0:
tie_list_ = tie_list_[same_color_filter_]
return tie_list_[0]
img = lab11_image.VectorImage("lab11_img1.yaml")
lines = img.lines
paths = []
paths_color = []
for path in img.paths:
ts = np.linspace(0, 1.0, 100)
result = np.empty((0, 3))
for i in range(0, path.num_segments()):
for t in ts[:-2]:
s = path.eval(i, t)
result = np.vstack([result, s])
paths.append(result)
paths_color.append(path.color)
