def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
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):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.pd_controller = PDController(500, 100, -75, 75)
# self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50)
self.pid_controller = PIDController(250, 30, 1, -100, 100, -100, 100)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.x_arr = np.array([])
self.y_arr = np.array([])
self.x_arr_truth = np.array([])
self.y_arr_truth = np.array([])
self.time_arr = np.array([])
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry(
robotProperties["diameter_left"],
robotProperties["diameter_right"],
robotProperties["wheel_base"],
robotProperties["encoder_count"]
)
# self.my_robot = MyRobot(None, self.create, self.time, self.odometry)
self.prev_r_count = 0
self.prev_l_count = 0
class Testloop:
def __init__(self):
self.odometry = Odometry()
self.BASE_SLEEP_TIME_US = 0.250 * 1000000
def _main_loop(self):
logs = ["" for _ in range(10000)]
log_pointer = 0
speed = 0
direction = 0
angle_l = 0
angle_r = 0
print('ready')
for _ in range(100):
start = datetime.datetime.now()
direction, speed = self.odometry.target_trace(angle_l, angle_r)
angle_l += 5
angle_r += 10
# 余った時間はsleep
elapsed_microsecond = (datetime.datetime.now() -
start).microseconds
if elapsed_microsecond < self.BASE_SLEEP_TIME_US:
sleep_time = (self.BASE_SLEEP_TIME_US -
elapsed_microsecond) / 1000000
time.sleep(sleep_time)
logs[log_pointer] = "{}, {}, {}, {}".format(
angle_l, angle_r, speed, direction)
log_pointer += 1
log_file = open("./log_test.csv", 'w')
for log in logs:
if log != "":
log_file.write("{}\n".format(log))
log_file.close()
#ログ記録用
self.odometry.shutdown(0000)
def __init__(self, mqtt_client, logger):
# Create Planet and planet_name variable
self.planet = Planet()
self.planet_name = None
# Setup Sensors, Motors and Odometry
self.rm = ev3.LargeMotor("outB")
self.lm = ev3.LargeMotor("outC")
self.sound = ev3.Sound()
self.odometry = Odometry(self.lm, self.rm)
self.motors = Motors(self.odometry)
self.cs = ColorSensor(self.motors)
self.us = Ultrasonic()
# Setup Communication
self.communication = Communication(mqtt_client, logger, self)
# Create variable to write to from communication
self.start_location = None
self.end_location = None
self.path_choice = None
self.running = True
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab10_map.Map("lab11.map")
self.odometry = Odometry()
def __init__(self, device_path):
self.device = serial.Serial(device_path, 115200, timeout=0.5)
self.write_lock = threading.Lock()
self.device.write(chr(128) * 3)
self.device.write(chr(131))
self.odometry = Odometry()
self.bumpers = [False, False]
self.drops = [False, False]
self.wall = False
self.cliff = [False, False, False, False]
self.vwall = False
self.dirt = 0
self.buttons = [False, False, False]
self.charging = 0
self.charge_current = 0
self.charge_voltage = 0
self._odom_left = 0
self._odom_right = 0
self._odom_last = [0, 0]
self._odom_start = True
self.joint_positions = [0, 0, 0, 0]
self.temperature = 0
self._packets = [
7, 8, 9, 10, 11, 12, 13, 14, 15, 18, 43, 44, 21, 22, 24, 25
]
self._i = 0
self._unmeasured = [False, False]
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.map = np.loadtxt("map14.txt", delimiter=",")
self.odometry = Odometry()
self.search = AStar(self.map)
self.location = (0,0)
self.orientation = "east"
self.pidTheta = pid_controller.PIDController(200, 25, 5, [-1, 1], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(100, 15, 5, [-10, 10], [-200, 200], is_angle=False)
self.base_speed = 50
def run():
# the execution of all code shall be started from within this function
print('Hello World')
planet = Planet()
odometry = Odometry()
movement_functions = Movement(motor_list = (ev3.LargeMotor('outB'), ev3.LargeMotor('outC')), odometry = odometry)
line_follower = follow.LineFollowing(colour_sensor = ev3.ColorSensor('in2'),
ts_list = (ev3.TouchSensor('in1'), ev3.TouchSensor('in4')), movement=movement_functions, odometry = odometry)
communication = Communication(planet = planet, odometry = odometry)
communication.connect()
line_follower.colour_calibration()
line_follower.line_following()
communication.first_communication()
sleep(2)
line_follower.path_recognising()
while True:
#odometry.odometry_calculations()
line_follower.line_following()
line_follower.path_recognising()
def serialize(dic):
if 'responses' in dic and 'frame_name' in dic:
responses = AnnotateResponse.serialize(dic['responses'])
env = None
odometry = None
width = None
height = None
depth = None
if 'odometry' in dic and dic['odometry'] != None:
odometry = Odometry.serialize(dic['odometry'])
if 'environment' in dic and dic['environment'] != None:
env = Environment.serialize(dic['environment'])
if 'image_width' in dic:
width = dic['image_width']
if 'image_height' in dic:
height = dic['image_height']
if 'image_depth' in dic:
depth = dic['image_depth']
return Responses(dic['frame_name'], responses, width, height, depth, odometry, env)
else:
return dic
print("X is horizontal, robot starts at 90 pose along +Y")
width = 21 #cm
print("Input coords are in grid coords")
start_x_g = float(input("Input the start x coord"))
start_y_g = float(input("Input the start y coord"))
goal_x_g = float(input("Input the desired x coord"))
goal_y_g = float(input("Input the desired y coord"))
# first implement move to point
# then implement hazard avoidance
dt = 0.02
odometry = Odometry(dt)
sensors.initSensors()
goal_location = Translation2d(goal_x_g * width, goal_y_g * width)
odometry.current_pos = RigidTransform2d(Translation2d(start_x_g*width, \
start_y_g*width), Rotation2d.fromDegrees(0))
# left A Right B
navigating = True
kP = 0.05
kMP = 0.003
resting_mag = 100
error_mag = 130
speed = 0.15
error_radius = 3 #cm
class Run:
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 run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.penholder.set_color(0.0, 1.0, 0.0)
# generate spline points and line drawing order
splines, splines_color = PathFinder.get_spline_points(self.img.paths)
# in format [index, is_parallel, is_spline]
line_index_list = PathFinder.find_path(self.img.lines, splines,
splines_color)
prev_color = None
# loop to draw all lines and paths
for draw_info in line_index_list:
index = int(draw_info[0])
is_parallel = draw_info[1]
is_spline = draw_info[2]
line = self.img.lines[index]
curr_color = self.img.lines[index].color
if curr_color != prev_color:
prev_color = curr_color
self.penholder.set_color(*get_color(curr_color))
# ===== spline routine =====
if is_spline:
path_points = self.draw_path_coords(splines[index],
is_parallel)
self.penholder.set_color(*get_color(splines_color[0]))
# go to start of the curve and begin drawing
for i in range(0, 2):
# go to start of curve
goal_x, goal_y = path_points[0, 0], path_points[0, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
if i == 1:
goal_x, goal_y = path_points[9, 0], path_points[9, 1]
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
if i == 1:
goal_theta = math.atan2(goal_y - path_points[0, 1],
goal_x - path_points[0, 0])
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
self.go_to_goal(goal_x, goal_y)
# start drawing after correctly oriented
# uses only odometry during spline drawing
self.penholder.go_to(self.penholder_depth)
prev_base_speed = self.base_speed
self.filter.updateFlag = False
self.base_speed = 25
print("Draw!")
last_drew_index = 0
# draw the rest of the curve. Draws every 10th point.
for i in range(10, len(path_points), 5):
goal_x, goal_y = path_points[i, 0], path_points[i, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=True)
print("\nlast drew index {}\n".format(last_drew_index))
if last_drew_index < len(path_points) - 1:
goal_x, goal_y = path_points[-1, 0], path_points[-1, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=False)
# stop drawing and restore parameter values
self.base_speed = prev_base_speed
self.filter.updateFlag = True
self.penholder.go_to(0.0)
# ===== line routine =====
else:
for i in range(0, 2):
goal_x, goal_y = self.draw_coords(line,
is_parallel=is_parallel,
at_start=True)
if i == 1:
goal_x, goal_y = self.draw_coords(
line, is_parallel=is_parallel, at_start=False)
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
if i == 1:
# start drawing
self.penholder.go_to(self.penholder_depth)
print("Draw!")
self.go_to_goal(goal_x, goal_y)
# graph the final result
self.draw_graph()
self.create.stop()
def drive(self, theta, distance, speed):
# Sum all controllers and clamp
self.create.drive_direct(
max(min(int(theta + distance + speed), 500), -500),
max(min(int(-theta + distance + speed), 500), -500))
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
self.update()
t = self.time.time()
if start + time_in_sec <= t:
break
# gives coordinates to draw the lines correctly
# line: segment to be drawn
# at_start: set true to retun the first coordinate, set false for the second coordinate
# returns the x, y coordinates offset
def draw_coords(self, line, at_start=True, is_parallel=True):
if is_parallel:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
def draw_path_coords(self, result, is_parallel):
final_result = np.empty((0, 2))
if is_parallel:
for i in range(0, len(result) - 1):
theta = math.atan2(
result[i, 1] - result[i + 1, 1],
result[i, 0] - result[i + 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
else:
for i in range(len(result) - 1, 1, -1):
theta = math.atan2(
result[i, 1] - result[i - 1, 1],
result[i, 0] - result[i - 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
return final_result
def go_to_goal(self, goal_x, goal_y, useOdo=False):
while True:
state = self.update()
if state is not None:
if useOdo:
curr_x = self.odometry.x
curr_y = self.odometry.y
curr_theta = self.odometry.theta
else:
curr_x = self.filter.x
curr_y = self.filter.y
curr_theta = self.filter.theta
distance = math.sqrt(
math.pow(goal_x - curr_x, 2) +
math.pow(goal_y - curr_y, 2))
output_distance = self.pidDistance.update(
0, distance, self.time.time())
theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
output_theta = self.pidTheta.update(curr_theta, theta,
self.time.time())
print("goal x,y = {:.3f}, {:.3f}, x,y = {:.3f}, {:.3f}".format(
goal_x, goal_y, curr_x, curr_y))
self.drive(output_theta, output_distance, self.base_speed)
if distance < 0.05:
self.create.drive_direct(0, 0)
break
self.sleep(0.01)
def go_to_angle(self, goal_theta):
curr_theta = self.filter.theta
while abs(-math.degrees(
math.atan2(math.sin(curr_theta - goal_theta),
math.cos(curr_theta - goal_theta)))) > 8:
curr_theta = self.filter.theta
print("goal_theta = {:.2f}, theta = {:.2f}".format(
math.degrees(goal_theta), math.degrees(curr_theta)))
output_theta = self.pidTheta.update(curr_theta, goal_theta,
self.time.time())
self.drive(output_theta, 0, 0)
self.sleep(0.01)
self.create.drive_direct(0, 0)
# debug function. Draws robot paths
def draw_graph(self):
# show drawing progress after each line segment is drawn
if self.debug_mode:
if len(self.odo) is not 0 and len(self.actual) is not 0:
x, y = zip(*self.odo)
a, b = zip(*self.actual)
plt.plot(x, y, color='red', label='Sensor path')
plt.plot(a,
b,
color='green',
label='Actual path',
linewidth=1.4)
self.odo = []
self.actual = []
for path in self.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])
plt.plot(result[:, 0], result[:, 1], path.color)
path_points = self.draw_path_coords(result, True)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
path_points = self.draw_path_coords(result, False)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
for line in self.img.lines:
# draw lines
plt.plot([line.u[0], line.v[0]], [line.u[1], line.v[1]],
line.color)
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
plt.legend()
plt.show()
# updates odometry, filter, and tracker
def update(self):
state = self.create.update()
self.filter.update()
# self.tracker.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
if self.debug_mode:
self.odo.append((self.filter.x, self.filter.y))
self.actual.append(
(self.create.sim_get_position()[0] - self.xi,
self.create.sim_get_position()[1] - self.yi))
return state
from frame_loader import FrameLoader
from odometry import FeatureExtractor
from odometry import Odometry
from visualizator import Visualizator
# Declarations
frame_loader = FrameLoader("data/video.mp4")
extractor = FeatureExtractor("orb")
visual_odometry = Odometry()
visualizator = Visualizator()
frame_read = True
while frame_read:
# Load and display the frame
frame_read, frame = frame_loader.get_frame()
# Extract features and descriptors
kp, des = extractor.extract_features(frame)
# Visualize results
visualizator.show_image(frame)
visualizator.show_keypoints(frame, kp)
# Printouts
print("Captured frame: %d" % frame_loader.frame_num)
print("Frame shape: %s" % (frame.shape,))
print("Extracted keypoints: %d" % len(kp))
frame_loader.close()
from odometry import Odometry
from transforms import *
import sensors
import time
import math
print("CCW is the positive direction!")
angle = float(input("Input the desired turn angle-> "))
dt = 0.01
odometry = Odometry(dt)
sensors.initSensors()
heading_setpoint = Rotation2d.fromDegrees(0)
# left A Right B
def setHeading(offset):
global heading_setpoint
sensors.updateSensors()
deg = odometry.getFieldToVehicle().getRotation().getDegrees()
heading_setpoint = Rotation2d.fromDegrees(deg).rotateBy(Rotation2d.fromDegrees(offset))
print("Heading set to: " + str(heading_setpoint))
def getHeadingError():
current_heading = odometry.getFieldToVehicle().getRotation()
return current_heading.inverse().rotateBy(heading_setpoint).getDegrees()
turn_speed = 0.15
sensors.setLeftMotor(-turn_speed * angle / math.fabs(angle))
sensors.setRightMotor(turn_speed * angle / math.fabs(angle))
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.pd_controller = PDController(500, 100, -75, 75)
# self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50)
self.pid_controller = PIDController(250, 30, 1, -100, 100, -100, 100)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
goal_angle = np.pi / 2
goal_angle %= 2 * np.pi
base_speed = 0
timeout = abs(17 * (goal_angle / np.pi)) + 1
angle = self.odometry.theta
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
start_time = self.time.time()
while self.time.time() - start_time < timeout:
angle = self.odometry.theta
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
# print("angle =%f, output = %f" % (np.rad2deg(angle), output))
# print("[r = %f, l = %f]\n" % (int(base_speed + output), int(base_speed - output)))
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
if not bad_data:
predictions.append([i, j, k])
errors.append(math.log(error))
if len(errors) > 0:
ix = errors.index(min(errors))
return predictions[ix]
return -1
if __name__ == '__main__':
init()
step()
odometry = Odometry(ENCODER_UNIT * (positionLeft.getValue()),
ENCODER_UNIT * (positionRight.getValue()), INIT_X,
INIT_Y, INIT_ANGLE)
count = 0
while (True):
odometry_info = odometry.track_step(
ENCODER_UNIT * (positionLeft.getValue()),
ENCODER_UNIT * (positionRight.getValue()))
if not step():
# print('saving data')
data_collector.collect(x_odometry, y_odometry, theta_odometry, x,
y, theta, np.array(distance_sensors_info))
plot()
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.odometry.x = 1
self.odometry.y = 0.5
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.travel_dist = 0.25
self.min_dist_to_wall = self.travel_dist + 0.2
self.min_dist_to_localize = 0.2
self.min_theta_to_localize = math.pi / 4
self.n_threshold = math.log(0.09)
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height,
input_map=self.map,
n_threshold=self.n_threshold)
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
isLocalized = False
angle_counter = 0
# This is an example on how to detect that a button was pressed in V-REP
while not isLocalized:
self.draw_particles()
# generate random num
command = random.choice([c for c in Command])
if command is Command.FORWARD:
if self.sonar.get_distance() < self.min_dist_to_wall:
continue
self.filter.move(0, self.travel_dist)
# 100 mm/s = 0.1 m/s
self.create.drive_direct(self.base_speed, self.base_speed)
self.sleep(abs(self.travel_dist / 0.1))
# stop
self.create.drive_direct(0, 0)
elif command is Command.TURN_LEFT:
# turn_angle = math.pi / 2 + self.odometry.theta
turn_angle = math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter += math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn left by 90 degree
self.go_to_angle(turn_angle)
elif command is Command.TURN_RIGHT:
# turn_angle = -math.pi / 2 + self.odometry.theta
turn_angle = -math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter -= math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn right by 90 degree
self.go_to_angle(turn_angle)
self.filter.sense(self.sonar.get_distance())
# check if localized
# distance between odometry and estimated positions
dist_position_to_goal = math.sqrt(
(self.odometry.x - self.filter.x)**2 +
(self.odometry.y - self.filter.y)**2)
diff_theta_to_goal = abs(self.odometry.theta - self.filter.theta)
isLocalized = dist_position_to_goal < self.min_dist_to_localize and diff_theta_to_goal < self.min_theta_to_localize
def go_to_angle(self, goal_theta):
while abs(
math.atan2(math.sin(goal_theta - self.odometry.theta),
math.cos(goal_theta - self.odometry.theta))) > 0.1:
# print("Go TO: " + str(math.degrees(goal_theta)) + " " + str(math.degrees(self.odometry.theta)))
output_theta = self.pidTheta.update(self.odometry.theta,
goal_theta, self.time.time())
self.create.drive_direct(int(+output_theta), int(-output_theta))
self.sleep(0.01)
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def draw_particles(self):
data = []
# get position data from all particles
for particle in self.filter.particles:
data.extend([particle.x, particle.y, 0.1, particle.theta])
# paint all particles in simulation
self.virtual_create.set_point_cloud(data)
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab10_map.Map("lab11.map")
self.odometry = Odometry()
def showParticles(self, particles):
self.virtual_create.set_point_cloud(particles)
def visualizePose(self,x,y,z,theta):
self.virtual_create.set_pose((x, y, z), theta)
def turnLeftOneSec(self):
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
def redrawParticles(self):
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
def findAndGoOnClearPath(self):
#Find new angle to move at
foundNewAngle = False
foundStartTime = False
runTime = 0
#Turn robot until clear space is found
while not foundNewAngle:
#Turn robot
self.turnLeftOneSec()
self.redrawParticles()
distanceFromNearestObstacle = self.sonar.get_distance()
obstacleAhead = distanceFromNearestObstacle < 0.001
if not obstacleAhead:
if not foundStartTime:
foundStartTime = True
else:
runTime += 1
if foundStartTime and runTime >= 4:
foundNewAngle = True
elif obstacleAhead and runTime < 4:
foundStartTime = False
runTime = 0
#Now turn the other way
turnRightTime = int(runTime / 2)
for i in range(0, turnRightTime):
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
#Actually move forward
self.create.drive_direct(50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.MoveForward()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to visualize the pose of our estimated position
# where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
#self.visualizePose(0.5, 0.5, 0.1, math.pi)
# This is an example on how to show particles
# the format is x,y,z,theta,x,y,z,theta,...
#data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
#self.showParticles(data)
# This is an example on how to estimate the distance to a wall for the given
# map, assuming the robot is at (0, 0) and has heading math.pi
#print(self.map.closest_distance((0.5,0.5), math.pi))
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
#This is an example on how to use PF
variances = [0.01,0.01,0.3]
self.pf = PF([self.map.bottom_left[0], self.map.top_right[0]],[self.map.bottom_left[1], self.map.top_right[1]],[0,2*math.pi], variances, self.map)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
# This is an example on how to detect that a button was pressed in V-REP
bNum = 0
while True:
self.create.drive_direct(0,0)
self.state = self.create.update()
if self.state:
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
prevX = self.odometry.x
prevY = self.odometry.y
prevTheta = self.odometry.theta
#b = self.virtual_create.get_last_button()
if self.pf.isOneCluster():
break
if bNum % 2 == 0:
b = self.virtual_create.Button.Sense
elif bNum % 4 == 1:
b = self.virtual_create.Button.TurnLeft
elif bNum % 4 == 3:
b = self.virtual_create.Button.MoveForward
bNum += 1
if b == self.virtual_create.Button.MoveForward:
print("Forward pressed!")
self.findAndGoOnClearPath()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnLeft:
print("Turn Left pressed!")
self.create.drive_direct(50,-50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnLeft()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.TurnRight:
print("Turn Right pressed!")
self.create.drive_direct(-50,50)
self.time.sleep(1)
self.create.drive_direct(0,0)
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.pf.TurnRight()
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
elif b == self.virtual_create.Button.Sense:
print("Sense pressed!")
dist = self.sonar.get_distance()
if dist:
self.pf.Sensing(dist)
print("Drawing particles")
for particle in self.pf.getParticles():
self.showParticles([particle.x,particle.y,0.1,particle.theta])
self.time.sleep(0.01)
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.x_arr = np.array([])
self.y_arr = np.array([])
self.x_arr_truth = np.array([])
self.y_arr_truth = np.array([])
self.time_arr = np.array([])
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
t = start
while t - start < time_in_sec:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
self.time_arr = np.append(self.time_arr, t)
self.x_arr = np.append(self.x_arr, self.odometry.x)
self.y_arr = np.append(self.y_arr, self.odometry.y)
self.x_arr_truth = np.append(self.x_arr_truth, self.create.sim_get_position()[0])
self.y_arr_truth = np.append(self.y_arr_truth, self.create.sim_get_position()[1])
t = self.time.time()
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
speed = 300 # mm/s
time_90deg_turn = 1.9 / (speed / 100.0) # seconds
# move forward for 1m
self.create.drive_direct(speed, speed)
self.sleep(1000 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 0.5m
self.create.drive_direct(speed, speed)
self.sleep(500 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 1m
self.create.drive_direct(speed, speed)
self.sleep(1000 / speed)
# turn left 90 deg
self.create.drive_direct(speed, -speed)
self.sleep(time_90deg_turn)
# move forward for 0.5m
self.create.drive_direct(speed, speed)
self.sleep(500 / speed)
# stop simulation
self.create.stop()
x_offset = self.x_arr_truth[np.argwhere(self.time_arr > 0.05)[0]]
y_offset = self.y_arr_truth[np.argwhere(self.time_arr > 0.05)[0]]
# plt.plot(self.time_arr, self.x_arr + x_offset, label='x coor measured')
# plt.plot(self.time_arr, self.x_arr_truth, '--', label='x coor truth')
# plt.plot(self.time_arr, self.y_arr + y_offset, label='y coor measured')
# plt.plot(self.time_arr, self.y_arr_truth, '--', label='y coor truth')
plt.plot(self.x_arr + x_offset, self.y_arr + y_offset, label='coor measured')
plt.plot(self.x_arr_truth, self.y_arr_truth, '--', label='coor truth')
plt.legend()
plt.grid()
plt.show()
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
def run(self):
# # This is an example on how to visualize the pose of our estimated position
# # where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi
# self.virtual_create.set_pose((0.5, 0.5, 0.1), 0)
#
# # This is an example on how to show particles
# # the format is x,y,z,theta,x,y,z,theta,...
# data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0]
# self.virtual_create.set_point_cloud(data)
#
# # This is an example on how to estimate the distance to a wall for the given
# # map, assuming the robot is at (0, 0) and has heading math.pi
# print(self.map.closest_distance((0.5,0.5), 0))
#
# # This is an example on how to detect that a button was pressed in V-REP
# while True:
# b = self.virtual_create.get_last_button()
# if b == self.virtual_create.Button.MoveForward:
# print("Forward pressed!")
# elif b == self.virtual_create.Button.TurnLeft:
# print("Turn Left pressed!")
# elif b == self.virtual_create.Button.TurnRight:
# print("Turn Right pressed!")
# elif b == self.virtual_create.Button.Sense:
# print("Sense pressed!")
#
# self.time.sleep(0.01)
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# This is an example on how to detect that a button was pressed in V-REP
while True:
self.draw_particles()
b = self.virtual_create.get_last_button()
if b == self.virtual_create.Button.MoveForward:
self.filter.move(0, 0.5)
# 100 mm/s = 0.1 m/s
self.create.drive_direct(self.base_speed, self.base_speed)
self.sleep(abs(0.5 / 0.1))
# stop
self.create.drive_direct(0, 0)
elif b == self.virtual_create.Button.TurnLeft:
turn_angle = math.pi / 2 + self.odometry.theta
turn_angle %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn left by 90 degree
self.go_to_angle(turn_angle)
elif b == self.virtual_create.Button.TurnRight:
turn_angle = -math.pi / 2 + self.odometry.theta
turn_angle %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn right by 90 degree
self.go_to_angle(turn_angle)
elif b == self.virtual_create.Button.Sense:
self.filter.sense(self.sonar.get_distance())
self.sleep(0.01)
def go_to_angle(self, goal_theta):
while abs(
math.atan2(math.sin(goal_theta - self.odometry.theta),
math.cos(goal_theta - self.odometry.theta))) > 0.01:
print("Go TO: " + str(math.degrees(goal_theta)) + " " +
str(math.degrees(self.odometry.theta)))
output_theta = self.pidTheta.update(self.odometry.theta,
goal_theta, self.time.time())
self.create.drive_direct(int(+output_theta), int(-output_theta))
self.sleep(0.01)
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def draw_particles(self):
data = []
# get position data from all particles
for particle in self.filter.particles:
data.extend([particle.x, particle.y, 0.1, particle.theta])
# paint all particles in simulation
self.virtual_create.set_point_cloud(data)
from ev3dev import ev3
from odometry import Odometry
from math import pi
odometry = Odometry(ev3.LargeMotor("outB"), ev3.LargeMotor("outC"), 6.0, 2.7,
6.0, 2 * pi / 5.0)
odometry.drive_to(-30.0, 0.0, 0)
odometry.drive_to(-30.0, 30.0, 0)
odometry.drive_to(-60.0, 60.0, pi)
def __init__(self):
self.odometry = Odometry()
self.BASE_SLEEP_TIME_US = 0.250 * 1000000
def guide(sh_mem, setting, log_datetime):
print('Im Guide')
def shutdown_child(signum=None, frame=None):
try:
sleep(0.2)
log_file = open("./log/log_{}_guide.csv".format(log_datetime), 'w')
for log in logs:
if log != "":
log_file.write("{}\n".format(log))
log_file.close()
if ('line_tracer' in globals()) or ('line_tracer' in locals()):
line_tracer.shutdown()
sys.exit()
except Exception as ex:
g_log.exception(ex)
raise ex
def wait_for_input():
print('Guide Waiting ...')
sh_mem.write_guide_is_ready_mem(1)
while not sh_mem.read_touch_sensor_mem():
sleep(0.1)
signal.signal(signal.SIGTERM, shutdown_child)
try:
# ここで変数定義などの事前準備を行う
# Time of each loop, measured in miliseconds.
loop_time_millisec = 18
# Time of each loop, measured in seconds.
loop_time_sec = loop_time_millisec / 1000.0
# ログ記録用
logs = ["" for _ in range(10000)]
log_pointer = 0
wait_for_input() # 設置が終わるまで待つ
line_tracer = LineTracer(setting)
# プログラム実行時にキャリブレーションを実行する場合は以下のコードを実行する
# print('Calibrate ColorSensor ...')
# line_tracer.calibrate_color_sensor()
print('Configurating Odometry ...')
odometry = Odometry()
print('Guide is Ready')
# タッチセンサー押し待ち
wait_for_input()
speed_reference = 0
direction = 0
odometry_speed_reference = 0
odometry_direction = 0
refrection_raw = 0
angle_l = 0
angle_r = 0
# スタート時の時間取得
t_line_trace_start = clock()
while True:
###############################################################
## Loop info
###############################################################
t_loop_start = clock()
# ここでライントレースする
speed_reference, direction, refrection_raw = line_tracer.line_tracing(
)
# 角度を算出してオドメトリーを使用
angle_l = sh_mem.read_motor_encoder_left_mem()
angle_r = sh_mem.read_motor_encoder_right_mem()
# odometry_speed_reference, odometry_direction = odometry.target_trace(angle_l,angle_r)
# direction = odometry_direction
# speed_reference = odometry_speed_reference
# 左右モーターの角度は下記のように取得
# print(read_motor_encoder_left_mem())
# print(read_motor_encoder_right_mem())
# 前進後退・旋回スピードは下記のように入力
sh_mem.write_speed_mem(speed_reference)
sh_mem.write_steering_mem(int(round(direction)))
# 実行時間、PID制御に関わる値をログに出力
t_loop_end = clock()
# logs[log_pointer] = "{}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}".format(
logs[log_pointer] = "{}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}".format(
t_loop_end - t_line_trace_start, t_loop_end - t_loop_start,
speed_reference, direction, refrection_raw,
line_tracer.refrection_target, line_tracer.e_b,
line_tracer.p_b, line_tracer.i_b, line_tracer.d_b, angle_l,
angle_r
# ,
# odometry.pre_pos_x,
# odometry.pre_pos_y,
# odometry_direction,
# odometry_speed_reference
)
log_pointer += 1
###############################################################
## Busy wait for the loop to complete
###############################################################
sleep(max(loop_time_sec - (clock() - t_loop_start), 0.002))
except Exception as e:
print("It's a Guide Exception")
g_log.exception(e)
shutdown_child()
from odometry import Odometry
from controller import WheeledRobotController
from math import pi
from ev3dev import ev3
from logger import Logger
odometry = Odometry(ev3.LargeMotor("outB"), ev3.LargeMotor("outC"), 6.0, 2.7,
10.0, 2 * pi / 5.0)
controller = WheeledRobotController(ev3.UltrasonicSensor("in2"),
ev3.UltrasonicSensor("in1"),
ev3.UltrasonicSensor("in3"), odometry)
logger = Logger("resources/map.csv")
controller.run_closed_loop(kp=0.025,
ki=0.0,
kd=0.012,
avg_n=4,
stop_threshold=5.0,
logger=logger,
delta=0.1)
logger.complete()
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.map = np.loadtxt("map14.txt", delimiter=",")
self.odometry = Odometry()
self.search = AStar(self.map)
self.location = (0,0)
self.orientation = "east"
self.pidTheta = pid_controller.PIDController(200, 25, 5, [-1, 1], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(100, 15, 5, [-10, 10], [-200, 200], is_angle=False)
self.base_speed = 50
def goToGoal(self, goal, state):
goal_y = goal[1] / 30
goal_x = goal[0] / 30
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
goal_theta = math.atan2(goal_y - self.odometry.y, goal_x - self.odometry.x)
theta = math.atan2(math.sin(self.odometry.x), math.cos(self.odometry.y))
output_theta = self.pidTheta.update(self.odometry.theta, goal_theta, self.time.time())
# improved version 2: fuse with velocity controller
self.distance = math.sqrt(math.pow(goal_x - self.odometry.x, 2) + math.pow(goal_y - self.odometry.y, 2))
output_distance = self.pidDistance.update(0, self.distance, self.time.time())
self.create.drive_direct(int(output_theta + output_distance)+self.base_speed, int(-output_theta + output_distance)+self.base_speed)
return output_distance
def shouldIContinueAlongMyPath(self):
#First scan ahead
distance = self.sonar.get_distance()
if distance < 0.5:
#Then add the obstacle to the map
if self.orientation == "east":
self.map[(self.location[0], self.location[1]+1)] = 1
elif self.orientation == "west":
self.map[(self.location[0], self.location[1]-1)] = 1
elif self.orientation == "north":
self.map[(self.location[0]-1, self.location[1])] = 1
else:
self.map[(self.location[0]+1, self.location[1])] = 1
return False
return True
def moveOneSquare(self, goal):
startX = self.odometry.x
startY = self.odometry.y
while True:
print("Moving")
#Get state and update the odometry
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.goToGoal(goal, self.state)) < 10:
break
if self.orientation == "east":
self.location = (self.location[0], self.location[1]+1)
elif self.orientation == "west":
self.location = (self.location[0], self.location[1]-1)
elif self.orientation == "south":
self.location = (self.location[0]+1, self.location[1])
else:
self.location = (self.location[0]-1, self.location[1]+1)
def turnCreate(self, goalTheta, state, pos):
self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts)
theta = math.atan2(math.sin(self.odometry.theta), math.cos(self.odometry.theta))
output_theta = self.pidTheta.update(self.odometry.theta, goalTheta, self.time.time())
# improved version 2: fuse with velocity controller
if pos:
self.create.drive_direct(int(output_theta), int(-output_theta))
else:
self.create.drive_direct(-int(output_theta), int(output_theta))
return output_theta
def turnTowardsPoint(self, nextPoint):
startTheta = self.odometry.theta
if nextPoint[0] < self.location[0]:
print("Turning north")
#Then turn north
goalTheta = math.pi / 2
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "north"
elif nextPoint[0] > self.location[0]:
print("Turning south")
#Then turn south
goalTheta = 3 * math.pi / 2
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "south"
elif nextPoint[1] < self.location[1]:
print("Turning west")
#Then turn west
goalTheta = math.pi
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "west"
else:
print("Turning east")
#Then turn east
goalTheta = 0
while True:
#Get state
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
if abs(self.turnCreate(goalTheta, self.state, True)) < 10:
break
print("Done turning")
self.orientation = "east"
def goOnPath(self, path):
for x, y in path:
if (x, y) == self.location:
pass
#Turn towards the point
print("Turning towards Point ", (x, y))
self.turnTowardsPoint((x,y))
#If point is now obstacle, go in a new path
if self.shouldIContinueAlongMyPath():
#Move towards point
print("No obstacle. Moving toward point")
self.moveOneSquare((x,y))
else:
return False
return True
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.state = self.create.update()
while not self.state:
self.state = self.create.update()
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
self.location = (9,1)#(self.odometry.x, self.odometry.y)
goal_pos = (1,9)
path = self.search.a_star(self.location, goal_pos)
print("Path is: ",path)
#Now go on that path
while not self.goOnPath(path):
path = self.search.a_star(self.location, goal_pos)
path.remove(self.location)
print("New Path is: ", path)
print("Done! I am at goal.")
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry(robotProperties["diameter_left"],
robotProperties["diameter_right"],
robotProperties["wheel_base"],
robotProperties["encoder_count"])
# self.my_robot = MyRobot(None, self.create, self.time, self.odometry)
self.x = 0.0
self.y = 0.0
self.angle = 0.0
self.prev_r_count = 0
self.prev_l_count = 0
def print_odometry(self, state_):
delta_r = self.odometry.get_delta_r(state_.rightEncoderCounts -
self.prev_r_count)
delta_l = self.odometry.get_delta_l(state_.leftEncoderCounts -
self.prev_l_count)
self.prev_r_count = state_.rightEncoderCounts
self.prev_l_count = state_.leftEncoderCounts
delta_theta = self.odometry.get_delta_theta(delta_r=delta_r,
delta_l=delta_l)
delta_d = self.odometry.get_delta_d(delta_r=delta_r, delta_l=delta_l)
print("[delta_r = %f, delta_l = %f, delta_d = %f, delta_theta = %f]" %
(delta_r, delta_l, delta_d, delta_theta))
self.x += delta_d * np.cos(self.angle)
self.y += delta_d * np.sin(self.angle)
self.angle += delta_theta
print("[x = %f, y = %f, angle = %f]\n" %
(self.x, self.y, np.rad2deg(self.angle)))
def run(self):
def move_robot(is_print: bool = False):
# move forward
self.my_robot.forward(5 * 0.1, is_print=is_print)
# left turn (in place)
self.my_robot.turn_left(2, is_print=is_print)
# wait
self.my_robot.wait(2, is_print=is_print)
# right turn (in place)
self.my_robot.turn_right(2, is_print=is_print)
# wait
self.my_robot.wait(2, is_print=is_print)
# move forward
self.my_robot.forward(2 * 0.1, is_print=is_print)
# move forward while turning
self.my_robot.move(0.2, 0.1, 7.5, is_print=is_print)
# move forward
self.my_robot.forward(5 * 0.1, is_print=is_print)
# move backwards
self.my_robot.backward(5 * 0.1, is_print=is_print)
# stop
self.my_robot.wait(3, is_print=is_print)
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
# move forward
while self.time.time() < 10:
self.create.drive_direct(50, 50)
state = self.create.update()
if state is not None:
print(state.__dict__)
self.print_odometry(state)
print("\n--------------reverse----------------")
start_time = self.time.time()
while self.time.time() - start_time < 10:
self.create.drive_direct(-50, -50)
state = self.create.update()
if state is not None:
print(state.__dict__)
self.print_odometry(state)
print("\n--------------turning----------------")
start_time = self.time.time()
while self.time.time() - start_time < 10:
self.create.drive_direct(50, -50)
state = self.create.update()
if state is not None:
print(state.__dict__)
self.print_odometry(state)
class Roomba:
def __init__(self, device_path):
self.device = serial.Serial(device_path, 115200, timeout=0.5)
self.write_lock = threading.Lock()
self.device.write(chr(128) * 3)
self.device.write(chr(131))
self.odometry = Odometry()
self.bumpers = [False, False]
self.drops = [False, False]
self.wall = False
self.cliff = [False, False, False, False]
self.vwall = False
self.dirt = 0
self.buttons = [False, False, False]
self.charging = 0
self.charge_current = 0
self.charge_voltage = 0
self._odom_left = 0
self._odom_right = 0
self._odom_last = [0, 0]
self._odom_start = True
self.joint_positions = [0, 0, 0, 0]
self.temperature = 0
self._packets = [
7, 8, 9, 10, 11, 12, 13, 14, 15, 18, 43, 44, 21, 22, 24, 25
]
self._i = 0
self._unmeasured = [False, False]
def send_drive(self, forward, rotate):
left = (forward - rotate * 0.28 / 2.0)
right = (forward + rotate * 0.28 / 2.0)
left *= 1000
right *= 1000
left = min(500, max(-500, left))
right = min(500, max(-500, right))
print(left, right)
self.device.write(struct.pack("!Bhh", 145, left, right))
def send_vacum(self, vacum, brush, side, side_clockwise):
bitfield = 0
bitfield += int(vacum) << 1
bitfield += int(brush) << 2
bitfield += int(side) << 0
bitfield += int(side_clockwise) << 3
self._unmeasured = [brush, side]
self.device.write(chr(138) + chr(bitfield))
def update_fake_joints(self, duration):
self.joint_positions[2] += duration * int(self._unmeasured[0]) * 12
self.joint_positions[3] += duration * int(self._unmeasured[1]) * 20
def send_leds(self, check, dock, spot, debris, cc, ci):
bitfield = int(check) << 3
bitfield += int(dock) << 2
bitfield += int(debris) << 1
bitfield += int(debris)
self.device.write(struct.pack("!BBBB", 139, bitfield, cc, ci))
def read_sensor(self):
self.device.write(chr(149))
self.device.write(chr(len(self._packets)))
for i in self._packets:
self.device.write(chr(i))
self._i = 0
rospy.sleep(0.005)
for i in self._packets:
self._read_data()
print("B")
def _read_data(self):
pid = self._packets[self._i]
self._i += 1
self._i %= len(self._packets)
if pid == 7:
dat = ord(self.device.read(1))
self.bumpers[1] = (dat & 0x1) != 0
self.bumpers[0] = (dat & 0x2) != 0
self.drops[1] = (dat & 0x4) != 0
self.drops[0] = (dat & 0x8) != 0
return 2
elif pid == 8:
self.wall = self.device.read(1) == chr(1)
return 2
elif 9 <= pid <= 12:
self.cliff[pid - 9] = self.device.read(1) == chr(1)
return 2
elif pid == 13:
self.vwall = self.device.read(1) == chr(1)
return 2
elif pid == 14 or pid == 16 or pid == 17:
self.device.read(1)
return 2
elif pid == 15:
self.dirt = ord(self.device.read(1)) / 255.0
return 2
elif pid == 18:
dat = ord(self.device.read(1))
self.buttons[0] = (dat & 0x1)
self.buttons[1] = (dat & 0x2)
self.buttons[2] = (dat & 0x4)
return 2
elif pid == 21:
self.charging = ord(self.device.read(1))
return 2
elif pid == 22:
self.charge_voltage = struct.unpack(
"!H", self.device.read(2))[0] / 1000.0
return 3
elif pid == 23:
self.device.read(2)
return 3
elif pid == 24:
self.temperature = struct.unpack("!b", self.device.read(1))[0]
return 2
elif pid == 25:
self.charge_current = struct.unpack(
"!H", self.device.read(2))[0] / 1000.0
return 3
elif pid == 26:
self.device.read(2)
return 3
elif pid == 43:
self._odom_left = struct.unpack("!h", self.device.read(2))[0]
return 3
elif pid == 44:
self._odom_right = struct.unpack("!h", self.device.read(2))[0]
if self._odom_start:
self._odom_start = False
self._odom_last = [self._odom_left, self._odom_right]
return
# conversion:
left = self._odom_left - self._odom_last[0]
right = self._odom_right - self._odom_last[1]
if left < -32768:
left += 65536
elif left > 32767:
left -= 65536
if right < -32768:
right += 65536
elif right > 32767:
right -= 65536
distance = (left + right) / 2
distance = (distance / encoder_counts) * 2 * math.pi
distance *= wheel_radius
angle = (
((right / encoder_counts) * 2 * math.pi * wheel_radius) -
((left / encoder_counts) * 2 * math.pi * wheel_radius)) / 0.28
print("ang", angle)
self.odometry.integrate(distance, angle)
self._odom_last = [self._odom_left, self._odom_right]
self.joint_positions[0] += (left / encoder_counts) * 2 * math.pi
self.joint_positions[1] += (right / encoder_counts) * 2 * math.pi
return 3
return 1
class Robot(object):
cycle_time = 0.01
enabled = False
odometry = Odometry(cycle_time)
def __init__(self, gui):
self.gui = gui
self.drive = Drive(self, gui)
def initialize(self):
sensors.initSensors()
def startLoop(self):
if (not self.enabled):
self.gui.log_message("Starting Main Robot Loop")
self.enabled = True
self.initialize()
self.drive.initDrive()
self.odometry.reset()
self.drive.setEnableIntersection(self.gui.getIntersectionEnabled())
self.drive.setEnableEntering(self.gui.getEnteringEnabled())
self.main_thread = threading.Thread(target=self.loop)
self.main_thread.start()
def stopLoop(self):
if (self.enabled):
self.gui.log_message("Stopping Main Robot Loop")
self.enabled = False
def loop(self):
loop_counter = 0
self.current = RigidTransform2d(Translation2d(0, 0),
Rotation2d.fromDegrees(0))
while (self.enabled):
loop_counter += 1
#self.gui.log_message("robot main")
# Able to decrease sensor update frequency
if (loop_counter % 1 == 0):
sensors.updateSensors()
self.odometry.updateOdometry()
self.drive.updateDrive()
if (loop_counter % 1 == 0):
self.current = self.odometry.getFieldToVehicle()
self.gui.log_pos(self.current)
self.gui.log_sonics(sensors.getLeftWallDistance(),
sensors.getForwardWallDistance(),
sensors.getRightWallDistance())
self.gui.log_mag(sensors.getMagneticMagnitude())
self.gui.log_ir(0.0, 0.0)
self.gui.log_state(self.drive.state)
if (loop_counter >= 1000):
loop_counter = 0
time.sleep(self.cycle_time)
sensors.shutdown()
class Run:
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = Odometry()
self.pd_controller = PDController(500, 100, -75, 75)
# self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50)
self.pid_controller = PIDController(500, 100, 10, -100, 100, -100, 100)
self.pid_controller_dist = PIDController(500, 100, 10, -100, 100, -100,
100)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
plt_goal_angle_arr = np.array([])
goal_x = -0.5
goal_y = -0.5
goal_coor = np.array([goal_x, goal_y])
goal_angle = np.arctan2(goal_y, goal_x)
goal_angle %= 2 * np.pi
print("goal angle = %f" % np.rad2deg(goal_angle))
base_speed = 100
# timeout = abs(17 * (goal_angle / np.pi)) + 1
goal_r = 0.05
angle = self.odometry.theta
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
while abs(dist_to_goal) > goal_r:
goal_angle = np.arctan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
goal_angle %= 2 * np.pi
print("(%f, %f), dist to goal = %f\n" %
(self.odometry.x, self.odometry.y, dist_to_goal))
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
angle = self.odometry.theta
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
output_dist = self.pid_controller_dist.update(
0, dist_to_goal, self.time.time())
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
# self.create.drive_direct(
# int((dist_to_goal * base_speed) + output),
# int((dist_to_goal * base_speed) - output)
# )
# self.create.drive_direct(
# int(output_dist + output),
# int(output_dist - output)
# )
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
np.savetxt("goalAngleOutput.csv", plt_goal_angle_arr, delimiter=",")
class Robot:
def __init__(self, mqtt_client, logger):
# Create Planet and planet_name variable
self.planet = Planet()
self.planet_name = None
# Setup Sensors, Motors and Odometry
self.rm = ev3.LargeMotor("outB")
self.lm = ev3.LargeMotor("outC")
self.sound = ev3.Sound()
self.odometry = Odometry(self.lm, self.rm)
self.motors = Motors(self.odometry)
self.cs = ColorSensor(self.motors)
self.us = Ultrasonic()
# Setup Communication
self.communication = Communication(mqtt_client, logger, self)
# Create variable to write to from communication
self.start_location = None
self.end_location = None
self.path_choice = None
self.running = True
def run(self):
counter = 0
bottle_detected = False
previous_l, previous_r, previous_b = 0, 0, 350
start_message = """
##################################
# #
# RoboLab Praktikum 2020 #
# Exploration by Paula #
# ❰❱ with ❤ by #
# Eric, Marc, Nico #
# #
##################################
"""
# Print start screen
print(start_message)
# Calibrate colors for better color accuracy
self.cs.calibrate_colors()
# Wait until we want to start
print("» Press Button to start")
sensors.button_pressed()
start_time = time.time()
print(time.strftime("Starttime: %H:%M:%S", time.localtime()))
print("Lets start to explore...")
# Start fancy features
feature_threads = features.start_features(self)
# Main robot loop
while self.running:
# Test if robot detect an obstacle
if self.us.get_distance() < 15:
# Rotate robot to drive back, and save that an obstacle was detected
self.sound.play("/home/robot/src/assets/found.wav")
print("Obstacle detected")
self.cs.rotate_to_path(180)
bottle_detected = True
continue
# Test if robot reached node
if self.cs.get_node() in ["blue", "red"] and counter == 0:
counter += 1
# Drive to center of node to better scanning
self.motors.drive_in_center_of_node(100, 2)
# Check if robot reached first node
if self.planet_name is None:
# Tell mothership to send planet information (planet_name and start coordinates)
self.communication.send_ready()
# Wait until message is received
while self.planet_name is None:
pass
# Reset odometry of last path, not used for first path
self.odometry.reset_list()
# Current robot orientation
forward_dir = self.start_location[1]
# End-position and direction of incoming path
self.end_location = (self.start_location[0], (forward_dir + 180) % 360)
# Save path as blocked for better path calculation
self.planet.add_path(self.end_location, self.end_location, -1)
else:
# Test if path is known
if self.start_location[0] in self.planet.planet_dict \
and self.start_location[1] in self.planet.planet_dict[self.start_location[0]]:
# Get end node from planet dictionary
((x, y), send_dir, _) = self.planet.planet_dict[self.start_location[0]][self.start_location[1]]
# Reset odometry of last path, not used for first path
self.odometry.reset_list()
else:
# Calculate postion and direction from odometry
x, y, forward_dir = self.odometry.calculate_path(
self.start_location[1], bottle_detected, self.start_location[0][0], self.start_location[0][1])
# Direction facing back, used for path-message
send_dir = (forward_dir + 180) % 360
# Send path to mothership get real position and direction
self.communication.send_path(self.start_location, ((x, y), send_dir), bottle_detected)
# Wait until message is received
while self.end_location is None:
pass
# Position of current node
current_pos = self.end_location[0]
# Direction of robot
forward_dir = (self.end_location[1] + 180) % 360
# Test if robot already scanned node
if current_pos in self.planet.scanned_nodes:
# If scanned, wait if mothership sends information like target and pathUnveiled
time.sleep(2)
else:
# Scan node for posible directions
possible_dirs = self.cs.analyze(forward_dir)
# Save direction into unexlored edges if they aren't in the planet dictionary
for d in possible_dirs:
if current_pos not in self.planet.planet_dict \
or d not in self.planet.planet_dict[current_pos].keys():
self.planet.add_unexplored_edge(current_pos, d)
# Mark node as scanned
self.planet.scanned_nodes.append(current_pos)
# Test if robot reached the target
if current_pos == self.planet.target:
# Send targetReached to mothership
self.communication.send_target_reached("Target reached!")
# Wait until confirmation
while self.running:
pass
self.sound.play("/home/robot/src/assets/smb_stage_clear.wav")
continue
# Calculate next direction based on planet information and posible directions
next_dir = self.choose_dir()
# If no direction is found: Exloration completed
if next_dir == -1:
# Send explorationCompleted to mothership
self.communication.send_exploration_completed("Exloration completed!")
# Wait until confirmation
while self.running:
pass
self.sound.play("/home/robot/src/assets/metroid.wav")
continue
# Save new starting postion for next path
self.start_location = (self.end_location[0], next_dir)
# Rotate to new path
self.cs.select_new_path(forward_dir, next_dir)
# Reset variables
self.end_location = None
bottle_detected = False
self.odometry.reset_position()
else:
# Follow line and save last data for next tick
previous_l, previous_r, previous_b = self.motors.follow_line(self.cs, previous_l, previous_r, previous_b)
# new (better solution?) -> multiple times calles -> ticks_previous needed
counter = 0
print(time.strftime("Endtime: %H:%M:%S", time.localtime()))
delta = (time.time() - start_time) / 60
print("Timedelta: %.2f min" % delta)
for thread in feature_threads:
thread.join()
def choose_dir(self):
# Next direction, -1 = no direction found
choice = -1
# Test if target is set
if self.planet.target is not None:
# Calculate shortest path to target
shortest = self.planet.shortest_path(self.end_location[0], self.planet.target)
# Test if target is reachable
if shortest is not None:
# Select first direction to get to target
choice = shortest[0][1]
# Test if no direction is set (no target or target not reachable)
if choice == -1:
# Nodes which need be explored
incomplete_nodes = []
# Shortest distance to unexplored node
distance = math.inf
# Nearest node
nearest = None
# Collect all node, which have open paths
for node in self.planet.unexplored_edges.keys():
incomplete_nodes.append(node)
# Collect all node, which aren't scanned
for node in self.planet.get_nodes():
if node not in self.planet.scanned_nodes:
incomplete_nodes.append(node)
# Test if all nodes are explored
if len(incomplete_nodes) == 0:
return -1
# Calculate nearest open node
for node in incomplete_nodes:
cur = self.planet.shortest_distance(self.end_location[0], node)
if cur < distance:
distance = cur
nearest = node
# Test if no open node is reachable (explorationCompleted)
if distance == math.inf:
return -1
# Test if open node is reached, choose first open path
elif distance == 0:
choice = self.planet.unexplored_edges[nearest][0]
# Otherwise drive to open node (select first direction to get to open node)
else:
choice = self.planet.shortest_next_dir(self.end_location[0], nearest)
# Send pathSelect to mothership
self.communication.send_path_select((self.end_location[0], choice))
# Wait until message receive or 3sec (answer optional)
start_time = time.time()
while self.path_choice is None and time.time() - start_time < 3.0:
pass
# Test if mothership overwrites direction, use forced direction
if self.path_choice is not None:
choice = self.path_choice
print("Next direction: %s" % Direction(choice))
# Reset Variable
self.path_choice = None
# Play sound
self.sound.play("/home/robot/src/assets/codecover.wav")
return choice