def transit_test():
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
x_pos = float(input("Enter x_pos of destination"))
y_pos = float(input("Enter y_pos of destination"))
print(currentState.getCurrentPos())
input('start?')
dest = pp.Position(x_pos, y_pos)
path = create_path(currentState.currentPos, dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
done = False
while not done:
for command in commands:
print(str(command[2]))
direction = True
if command[0] < 0:
direction = False
okay = turn(math.fabs(command[0]), direction, ROBOT_SPEED_TURN)
if not okay:
break
distance = command[1]
next_x = currentState.getCurrentPos().getX(
) + math.fabs(distance) * math.cos(
currentState.getCurrentPos().getOrientation())
next_y = currentState.getCurrentPos().getY(
) + math.fabs(distance) * math.sin(
currentState.getCurrentPos().getOrientation())
next_pos = pp.Position(next_x, next_y)
direction = True
if distance < 0:
direction = False
okay = rpmdrive(next_pos, direction, distance, ROBOT_SPEED_DRIVE)
if not okay:
break
rospy.sleep(0.01)
if currentState.getCurrentPos().distanceTo(command[2]) > 0.2:
break
if currentState.getCurrentPos(
) == dest or currentState.getCurrentPos().getDistanceTo(dest) < 0.2:
done = True
else:
print("modifying path")
path = create_path(currentState.getCurrentPos(), dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
if rospy.is_shutdown():
exit(-1)
class Planning(object):
def __init__(self):
self._pp = PathPlanning()
# Subscriber to the start and goal tag ID (the two values are separated by a comma).
self._subStartGoal = rospy.Subscriber('start_goal_tag', String, self._handleStartGoal)
# Publisher of the path, which is given as a list of comma separated tag IDs.
self._pubPathTags = rospy.Publisher('path_tags', String, queue_size=10, latch=True)
# Publisher of the actions, which are given as a semicolon separated list of actions.
self._pubPathActions = rospy.Publisher('path_actions', String, queue_size=10, latch=True)
def _handleStartGoal(self, msg):
try:
startGoal = msg.data.split(',')
path = self._pp.findPath(int(startGoal[0]), int(startGoal[1]))
patha, pathb = tee(path)
msgPathTags = String()
msgPathTags.data = ','.join(str(x) for x in patha)
if msgPathTags.data != '':
self._pubPathTags.publish(msgPathTags)
actions = self._pp.generateActions(list(pathb))
msgPathActions = String()
msgPathActions.data = ';'.join('{:s},{:f},{:d}'.format(*x) for x in actions)
if msgPathActions.data != '':
self._pubPathActions.publish(msgPathActions)
except AttributeError:
print('Error! Something went wrong :(')
def __init__(self):
self._pp = PathPlanning()
# Subscriber to the start and goal tag ID (the two values are separated by a comma).
self._subStartGoal = rospy.Subscriber('start_goal_tag', String,
self._handleStartGoal)
# Publisher of the path, which is given as a list of comma separated tag IDs.
self._pubPathTags = rospy.Publisher('path_tags', String, queue_size=10)
def transit_test1():
print('simple path following stuff')
print('currentPos: ' + str(currentState.getCurrentPos()))
dest_x = float(input('enter destination x pos'))
dest_y = float(input('enter destination y pos'))
dest = pp.Position(dest_x, dest_y)
print(str(len(currentState.getObstacles().values())))
path = create_path(currentState.getCurrentPos(), dest, 4.2672, 6.096,
currentState.getObstacles().values())
send_path_data(path)
for position in path.path:
print(str(position))
exit(0)
commands = convertToCommands(path)
for command in commands:
if command[0] > 0:
turn_algo_2(command[0], True)
else:
turn_algo_2(math.fabs(command[0]), False)
dest_x = currentState.getCurrentPos().getX() + command[1] * math.cos(
currentState.getCurrentPos().getOrientation())
dest_y = currentState.getCurrentPos().getY() + command[1] * math.sin(
currentState.getCurrentPos().getOrientation())
dest = pp.Position(dest_x, dest_y,
currentState.getCurrentPos().getOrientation())
if command[1] > 0:
conservative_drive(dest, True, command[1])
else:
conservative_drive(dest, False, math.fabs(command[1]))
print('testing done')
exit(0)
def updateRelativePos(d_distance, d_angle):
pos = currentState.getCurrentPos()
if d_distance > 0:
x = pos.getX() + d_distance * math.cos(pos.getOrientation())
y = pos.getY() + d_distance * math.sin(pos.getOrientation())
currentState.setCurrentPos(pp.Position(x, y, pos.getOrientation()))
elif d_angle > 0:
next_pos = pp.Position(pos.getX(), pos.getY(),
(pos.getOrientation() + toRadian(d_angle)) %
(2 * math.pi))
currentState.setCurrentPos(next_pos)
def looky_turn(currentPos, next_distance):
nextX = currentPos.getX() + next_distance * math.cos(
currentPos.getOrientation())
nextY = currentPos.getY() + next_distance * math.sin(
currentPos.getOrientation())
nextPos = pp.Position(nextX, nextY, currentPos.getOrientation())
looky_turn_2(nextPos, COLLECTION_BIN)
def updateObstacle(msg):
global currentState
obs = pp.Obstacle(msg.x, msg.y, msg.diameter / 2)
if currentState.addObstacle(msg.obsID, obs):
print('added obstacle')
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
def simple_turn_test1():
rospy.loginfo('This test routine attempts to turn in place given degrees')
while True:
print('current position: ' + str(currentState.getCurrentPos()))
answer = input('Start testing? y/n ')
if answer == 'y':
break
goal = float(
input(
'Enter how much to turn in degrees. input should be less than 360')
)
goal_in_rad = toRadian(goal)
currentPos = currentState.getCurrentPos()
destination = pp.Position(currentPos.getX(), currentPos.getY(),
(currentPos.getOrientation() + goal_in_rad) %
(2 * math.pi))
direction = True
if goal > 0:
direction = True
else:
direction = False
turn_algo_2(math.fabs(goal), direction)
print('testing succesful')
rospy.loginfo('localization data:')
rospy.loginfo(
'start_angle:' + str(toDegree(currentPos.getOrientation())) +
' end_angle' +
str(toDegree(currentState.getCurrentPos().getOrientation())) +
' actual_goal:' + str(toDegree(destination.getOrientation())))
exit(0)
def simple_drive_test1():
rospy.loginfo(
'This test routine attempts to drive straight from is current position'
)
distance = float(input('Enter distance you want to travel'))
while True:
if rospy.is_shutdown():
exit(-1)
print('current position: ' + str(currentState.getCurrentPos()))
answer = input(
'Make sure that robot is in a place okay to drive forward. Start testing? y/n '
)
if answer == 'y':
break
currentPos = currentState.getCurrentPos()
destination = pp.Position(
currentPos.getX() + distance * math.cos(currentPos.getOrientation()),
currentPos.getY() + distance * math.sin(currentPos.getOrientation()),
currentPos.getOrientation())
direction = True
if distance > 0:
direction = True
else:
direction = False
conservative_drive(destination, direction, math.fabs(distance))
measure = float(input('How much did it move? (in m): '))
rospy.loginfo('distance_travelled:' + str(measure) + ' distance_entered:' +
str(distance))
print('testing successful')
exit(0)
def main():
lenX = 101
lenY = 101
_maze = MazeGenerator([lenX, lenY])
_maze.constructNotRecursion()
maze = txtToMatrix("mazeTxt.txt")
node_start = Node([0, 1])
node_start.gcost = 0
node_goal = Node([lenX - 1, lenY - 2])
path = PathPlanning(maze, node_start, node_goal)
route = path.DepthFirstSearch()
desenho = Maze(txtToMatrix("mazeTxt.txt"))
desenho.draw(route, False)
def converToCommands(path):
commands = []
first_pos = True
currentPos = currentState.getCurrentPos()
for position in path.path:
if first_pos:
first_pos = False
else:
angle_to_face = currentPos.angleToFace(position)
pos = pp.Position(currentPos.getX(), currentPos.getY(),
angle_to_face)
angle_turn = currentPos.angleTurnTo(pos)
distance = currentPos.distanceTo(position)
commands.append((toDegree(angle_turn), distance, position))
currentPos = pp.Position(
pos.getX() + distance * math.cos(angle_to_face),
pos.getY() + distance * math.sin(angle_to_face), angle_to_face)
currentPos.orientation = angle_to_face
return commands
def looky_turn(currentPos, next_distance):
nextX = currentPos.getX() + next_distance * math.cos(
currentPos.getOrientation())
nextY = currentPos.getY() + next_distance * math.sin(
currentPos.getOrientation())
nextPos = pp.Position(nextX, nextY, currentPos.getOrientation())
looky_angle = (nextPos.angleToFace(pp.Position(0, 0, 0))) % (2 * math.pi)
if looky_angle >= (toRadian(-150) % 2 *
math.pi) or looky_angle <= toRadian(150):
if looky_angle <= toRadian(150):
mc.star_looky(motor_pub, toDegree(looky_angle))
else:
mc.star_looky(motor_pub, toDegree(looky_angle - 2 * math.pi))
elif looky_angle >= ((toRadian(-150) + math.pi) % (2 * math.pi)) \
and looky_angle <= ((toRadian(150) + math.pi) % (2 * math.pi)):
mc.port_looky(motor_pub, toDegree(looky_angle - math.pi))
else:
mc.star_looky(motor_pub, 150)
mc.port_looky(motor_pub, -150)
class Planning(object):
def __init__(self):
self._pp = PathPlanning()
# Subscriber to the start and goal tag ID (the two values are separated by a comma).
self._subStartGoal = rospy.Subscriber('start_goal_tag', String,
self._handleStartGoal)
# Publisher of the path, which is given as a list of comma separated tag IDs.
self._pubPathTags = rospy.Publisher('path_tags', String, queue_size=10)
def _handleStartGoal(self, msg):
startGoal = msg.data.split(',')
path = self._pp.findPath(int(startGoal[0]), int(startGoal[1]))
msgPathTags = String()
msgPathTags.data = ','.join(str(x) for x in path)
self._pubPathTags.publish(msgPathTags)
def rpm_drive_test():
print("How much distance you want to move?")
distance = float(input())
direction = True
if distance < 0:
direction = False
dest_x = currentState.getCurrentPos().getX() + math.fabs(
distance) * math.cos(currentState.getCurrentPos().getOrientation())
dest_y = currentState.getCurrentPos().getY() + math.fabs(
distance) * math.sin(currentState.getCurrentPos().getOrientation())
dest = pp.Position(dest_x, dest_y)
rpmdrive(dest, direction, distance, ROBOT_SPEED_DRIVE)
result = float(input("how much did it go?"))
rospy.loginfo("goal:" + str(distance) + " actual:" + str(result) + "\n")
exit(0)
def looky_turn_2(currentPos, next_pos):
looky_angle = (currentPos.angleToFace(pp.Position(0, 0,
0))) % (2 * math.pi)
print(str(looky_angle))
if looky_angle >= (toRadian(-150) %
(2 * math.pi)) or looky_angle <= toRadian(150):
if looky_angle <= toRadian(150):
mc.star_looky(motor_pub, toDegree(looky_angle))
else:
mc.star_looky(motor_pub, toDegree(looky_angle - 2 * math.pi))
elif looky_angle >= ((toRadian(-150) + math.pi) % (2 * math.pi)) \
and looky_angle <= ((toRadian(150) + math.pi) % (2 * math.pi)):
mc.port_looky(motor_pub, toDegree(looky_angle - math.pi))
else:
mc.star_looky(motor_pub, 150)
mc.port_looky(motor_pub, -150)
def updateObstacle(msg):
global currentState
if currentState.currentPos is None or currentState.ignore_obs:
return
robot_orient = currentState.getCurrentPos().getOrientation()
true_dx = msg.y * math.cos(robot_orient) + msg.x * math.cos(robot_orient -
math.pi / 2)
true_dy = msg.y * math.sin(robot_orient) + msg.x * math.sin(robot_orient -
math.pi / 2)
rad_clear = min(msg.diameter / 2 + 0.75,
math.sqrt(true_dx**2 + true_dy**2) - 0.25)
obs = pp.Obstacle(currentState.getCurrentPos().getX() + true_dx,
currentState.getCurrentPos().getY() + true_dy, rad_clear)
if obs.getCenter()[0] < 0.25 or obs.getCenter(
)[0] > ARENA_WIDTH - 0.25 or obs.getCenter(
)[1] < 2.74 + 0.25 or obs.getCenter()[1] > 1.9 + 2.74 - 0.25:
return
if currentState.addObstacle(msg.obsID, obs):
print('saw obstacle')
mc.drive_left_motor(motor_pub, 0)
mc.drive_right_motor(motor_pub, 0)
def turn_algo_2(goal, counter):
offset = currentState.getGyroZ()
cum_angle = 0
stop_angle = 0
flag = False
done = False
initTime = time.time()
lastTime = None
goal_orientation = (currentState.getCurrentPos().getOrientation() +
toRadian(goal)) % (2 * math.pi)
goal_pos = pp.Position(currentState.getCurrentPos().getX(),
currentState.getCurrentPos().getY(),
goal_orientation)
looky_turn_2(currentState.getCurrentPos(), goal_pos)
if counter:
mc.drive_left_motor(motor_pub, -ROBOT_SPEED_TURN)
mc.drive_right_motor(motor_pub, ROBOT_SPEED_TURN)
else:
mc.drive_left_motor(motor_pub, ROBOT_SPEED_TURN)
mc.drive_right_motor(motor_pub, -ROBOT_SPEED_TURN)
while not done:
w = currentState.getGyroZ() - offset
if not flag:
if math.fabs((math.fabs(currentState.getStarRPM()) +
math.fabs(currentState.getPortRPM())) / 2 -
ROBOT_SPEED_TURN) < 1:
print('hi')
stop_angle = goal - cum_angle
print(str(stop_angle))
flag = True
elif cum_angle >= goal / 2:
stop_angle = cum_angle
flag = True
if lastTime is None:
lastTime = time.time()
else:
currentTime = time.time()
deltaT = currentTime - lastTime
cum_angle += math.fabs(w) * deltaT
lastTime = currentTime
logGyroData(currentState.getGyroZ() + offset,
currentState.getPortRPM(), currentState.getStarRPM(),
cum_angle, currentTime - initTime)
if flag and cum_angle >= stop_angle:
mc.drive_right_motor(motor_pub, 0)
mc.drive_left_motor(motor_pub, 0)
done = True
if rospy.is_shutdown():
exit(-1)
while currentState.getGyroZ() > 1:
w = currentState.getGyroZ() - offset
currentTime = time.time()
cum_angle += math.fabs(w * (currentTime - lastTime))
lastTime = currentTime
rospy.sleep(0.005)
measure = input('How much did it turn? (in deg): ')
logTurnData(counter, ROBOT_SPEED_TURN, goal, cum_angle, float(measure),
lastTime - initTime)
def run():
dest = pp.Position(1.5, 4.0)
path = create_path(currentState.currentPos, dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
if path is None:
currentState.ignore_obs = True
currentState.obstacles = {}
path = create_path(currentState.currentPos, dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
done = False
while not done:
for command in commands:
print(str(command[2]))
direction = True
if command[0] < 0:
direction = False
okay = turn(math.fabs(command[0]), direction, ROBOT_SPEED_TURN)
print('finished turn')
if not okay:
break
distance = command[1]
next_x = currentState.getCurrentPos().getX(
) + math.fabs(distance) * math.cos(
currentState.getCurrentPos().getOrientation())
next_y = currentState.getCurrentPos().getY(
) + math.fabs(distance) * math.sin(
currentState.getCurrentPos().getOrientation())
next_pos = pp.Position(next_x, next_y)
direction = True
if distance < 0:
direction = False
okay = rpmdrive(next_pos, direction, distance, ROBOT_SPEED_DRIVE)
if not okay:
break
rospy.sleep(0.01)
if currentState.getCurrentPos().distanceTo(command[2]) > 0.2:
break
if currentState.getCurrentPos(
) == dest or currentState.getCurrentPos().distanceTo(dest) < 0.2:
done = True
else:
currentState.obstacle_found = False
print("modifying path")
path = create_path(currentState.getCurrentPos(), dest, ARENA_WIDTH,
ARENA_HEIGHT,
currentState.getObstacles().values())
if path is None:
currentState.ignore_obs = True
currentState.obstacles = {}
path = create_path(currentState.getCurrentPos(), dest,
ARENA_WIDTH, ARENA_HEIGHT,
currentState.getObstacles().values())
commands = converToCommands(path)
print(str(commands))
if rospy.is_shutdown():
exit(-1)
def updatePos(msg):
global currentState
pos = pp.Position(msg.x, msg.y, msg.theta)
if not currentState.oriented:
currentState.setCurrentPos(pos)
currentState.foundTag()
def updatePos(msg):
global currentState
pos = pp.Position(msg.x, msg.y, msg.theta)
currentState.setCurrentPos(pos)
def convert_axis(pos, dx, dy):
return pp.Position(pos.getY() + dy, dx - pos.getX(),
(pos.getOrientation() + 3 * math.pi / 2) %
(2 * math.pi))
from PathPlanning.ThetaStar import create_path
# from PathPlanning.PathTesting import drawPath
from RobotState import Robot_state
from apriltags_ros.msg import Localization
from hci.msg import sensorValue, motorCommand
from obstacle_detection.msg import Obstacle
from std_msgs.msg import Float32
"""
Main Module for Autonomy operation
"""
"""
Constants
"""
ARENA_WIDTH = 3.0
ARENA_HEIGHT = 7.0
COLLECTION_BIN = pp.Position(0, 0)
ROBOT_SPEED_DRIVE = 20.0
ROBOT_SPEED_DRIVE_LOAD = 30.0
ROBOT_SPEED_TURN = 15.0
ROBOT_SPPED_TURN_LOAD = 25.0
CONTROL_RATE = 0.005
WHEEL_RADIUS = 0.2286 # IN M
TURN_RADIUS = 0.3 # IN M
# Map from ID to time allocated.
tasks = {
0: -1,
1: 20,
2: 20,
3: 50,
4: 100,
5: 20,