def __init__(self, hw = None, configfile="config", L=0.14,
max_speed_hw=90, max_steer_hw=40, unit_converter=None,
virtual=False, verbose=False, virtual_verbose=False):
self.verbose = verbose
# Hardware
if hw is None:
if unit_converter is None:
unit_converter = Converter(speed_slope=1, angle_slope=1) # default to hardware units = world units
hw = Hardware(max_steer=max_steer_hw, max_speed=max_speed_hw,
unit_converter=unit_converter, configfile=configfile,
virtual=virtual, virtual_verbose=virtual_verbose)
self.hw = hw
self.L = L
# Control State
self._current_speed = 0
self._current_steer = 0
self._current_direction = 1 # Forward
# Model
self._world_pose = CartesianPose(0,0,0)
self._goal_pose = CartesianPose(0,0,0)
self._bicycle_pose = BicyclePose(0,0,0)
def __init__(
self,
max_turn=45,
configfile="config",
L=0.145,
# kpr=1, kpa=0, kpb=0, kdr=0, kda=0, kdb=0, kir=0, kia=0, kib=0,
# controllers=lambda x: x, min_dt=0.005,
max_picar_speed=80,
max_picar_turn=40,
virtual=False,
verbose=False,
virtual_speedverbose=False):
self.verbose = verbose
# Use virtual libraries if requested (so we don't get GPIO errors for not running on a Pi)
if not virtual:
import picar
from picar.front_wheels import Front_Wheels
from picar.back_wheels import Back_Wheels
picar.setup()
self._fw = Front_Wheels(db=configfile)
self._bw = Back_Wheels(db=configfile)
else:
from virtual_wheels import Virtual_Front_Wheels as Front_Wheels
from virtual_wheels import Virtual_Back_Wheels as Back_Wheels
self._fw = Front_Wheels(db=configfile,
verbose=virtual_speedverbose)
self._bw = Back_Wheels(db=configfile, verbose=virtual_speedverbose)
# Wheel base [m]
self._L = L
# Max turn radius
self._fw.max_turn = max_picar_turn
# Initialize wheels
self._speed = 0 # from 0-100
self._steering = 0 # initalize at no turn angle
self._direction = 1 # wheels set forward
self._fw.ready()
self._bw.ready()
self._bw.forward()
# Controllers for calculating control signals
# self.controller = MyPicarController(v_controllers, gamma_controllers)
# self._rhomyPID = myPID(Kp=kpr, Ki=kir, Kd=kdr)
# self._alphamyPID = myPID(Kp=kpa, Ki=kia, Kd=kda)
# self._betamyPID = myPID(Kp=kpb, Ki=kib, Kd=kdb)
# Set maximum values of control signals -- IN PICAR UNITS (e.g. degrees, not radians)
self.MAX_PICAR_SPEED = max_picar_speed
self.MAX_PICAR_TURN = max_picar_turn
# Minimum loop delay
# self.min_dt = min_dt
# Initialize WorldState and BicycleModel
self._my_cartesian_pose = CartesianPose(0, 0, 0)
self._goal_cartesian_pose = CartesianPose(0, 0, 0)
self._my_bicycle_model = BicycleModel(rho=0, alpha=0, beta=0)
def next_picar_pose(self, speed, steer, dt, direction=1, picar_pose=None):
if picar_pose is None:
picar_pose = self.picar_pose
dx = dXdt(speed=speed, heading=picar_pose.h, direction=direction) * dt
dy = dYdt(speed=speed, heading=picar_pose.h, direction=direction) * dt
dh = dHdt(speed=speed,
steer=steer,
direction=direction,
L=self.picar_wheelbase) * dt
return picar_pose + CartesianPose(dx, dy, dh)
def __init__(self,
xlim=None,
ylim=None,
obstacles=None,
obstacle_val=1,
resolution=1,
costmap=None,
picar_pose=CartesianPose(0, 0, 0),
goal_pose=CartesianPose(0, 0, 0),
waypoints=None,
picar_wheelbase=0.14,
qr_codes=None):
'''
@TODO: Environment stuff (lims, costmap, obstacles, etc.) yet to be implemented -- just placeholder arguments for now.
'''
if xlim is None:
xlim = [0, 10]
if ylim is None:
ylim = xlim # Default to square map
if costmap is None:
costmap = Map(origin=[0, 0],
xlim=xlim,
ylim=ylim,
resolution=resolution,
fill=0)
if obstacles is not None:
for obs in obstacles: # obstacles should be a list of Rects
costmap.fill_rect(obs, fill=obstacle_val)
if waypoints is None:
waypoints = [goal_pose]
self.xlim = xlim
self.ylim = ylim
self.costmap = costmap
self.obstacles = obstacles
self.qr_codes = qr_codes
self.picar_pose = picar_pose
self.goal_pose = goal_pose
self.waypoints = waypoints
self.picar_wheelbase = picar_wheelbase
def bicycle2goal(bicycle_pose, robot_cartesian):
'''
Calculate goal_cartesian from bicycle model and robot_cartesian
'''
goal_x = robot_cartesian.x + bicycle_pose.rho * cos(robot_cartesian.h +
bicycle_pose.alpha)
goal_y = robot_cartesian.y + bicycle_pose.rho * sin(robot_cartesian.h +
bicycle_pose.alpha)
goal_h = robot_cartesian.h + bicycle_pose.alpha + bicycle_pose.beta
return CartesianPose(goal_x, goal_y, goal_h)
def bicycle2robot(bicycle_pose, goal_cartesian):
'''
Calculate robot_cartesian from bicycle model and goal_cartesian
'''
picar_x = goal_cartesian.x - bicycle_pose.rho * cos(goal_cartesian.h -
bicycle_pose.beta)
picar_y = goal_cartesian.y - bicycle_pose.rho * sin(goal_cartesian.h -
bicycle_pose.beta)
picar_h = goal_cartesian.h - bicycle_pose.beta - bicycle_pose.alpha
return CartesianPose(picar_x, picar_y, picar_h)
def set_goal_cartesian(self, goal=None, xyh=None):
'''
Set a new goal pose for the picar. Recalculate the BicyclePose based on this
new goal pose and the current picar pose.
Picar pose itself is not updated.
'''
if goal is None:
goal = CartesianPose(*xyh)
self._goal_cartesian_pose = goal
self._my_bicycle_model = pr.cartesian_to_bicycle(
goal, robot_in_cartesian_ref_frame=self._my_cartesian_pose)
def set_my_cartesian(self, pose=None, xyh=None):
'''
Set a new cartesian pose for the picar. Recalculate the BicyclePose based on this
new picar pose and the current goal pose.
Goal pose itself is not updated.
'''
if pose is None:
pose = CartesianPose(*xyh)
self._my_cartesian_pose = pose
self._my_bicycle_model = pr.cartesian_to_bicycle(
self._goal_cartesian_pose, robot_in_cartesian_ref_frame=pose)
def calculate_next_pose(old_pose, v, gamma, dt=1):
'''
Calculate the next CartesianPose state of an object with a certain speed and steering angle.
'''
# Calculate change in world state
dx = dX(v=v, h=old_pose.h)
dy = dY(v=v, h=old_pose.h)
dh = dH(v=v, gamma=gamma, L=L)
# Update world state
new_pose = old_pose + CartesianPose(xyh=[dx, dy, dh]) * dt
# Keep h in [-pi, pi]
new_pose.h = within_pi(new_pose.h)
return new_pose
def next_pose(self, speed, steer, direction, dt):
dxdt = dXdt(speed=speed, heading=self.h(), direction=direction)
dydt = dYdt(speed=speed, heading=self.h(), direction=direction)
dhdt = dHdt(speed=speed, steer=steer, direction=direction, L=self.L)
return self._world_pose+CartesianPose(dxdt,dydt,dhdt)*dt