def gotoRegion(self, current_reg, next_reg, last=False):
"""
If ``last`` is True, we will move to the center of the destination region.
Returns ``True`` if we've reached the destination region.
"""
q_gBundle = [[], []] # goal bundle
q_overlap = [[], []] # overlapping points with robot range
pose = self.pose_handler.getPose() # Find our current configuration
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.original_figure)
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Check if Vicon has cut out
if math.isnan(pose[2]):
print "no vicon pose"
print "WARNING: No Vicon data! Pausing."
#self.drive_handler.setVelocity(0, 0) # So let's stop
time.sleep(1)
return False
###This part is run when the robot goes to a new region, otherwise, the original map will be used.
if not self.previous_current_reg == current_reg:
#print 'getting into bug alogorithm'
#clean up the previous self.map_work
self.map_work = Polygon.Polygon()
# NOTE: Information about region geometry can be found in self.proj.rfi.regions
# create polygon list for regions other than the current_reg and the next_reg
self.map_work += self.map[self.proj.rfi.regions[current_reg].name]
self.map_work += self.map[self.proj.rfi.regions[next_reg].name]
# building current polygon and destination polygon
self.nextRegionPoly = self.map[
self.proj.rfi.regions[next_reg].name]
self.currentRegionPoly = self.map[
self.proj.rfi.regions[current_reg].name]
#set to zero velocity before finding the tranFace
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
print "Current reg is " + str(
self.proj.rfi.regions[current_reg].name.lower())
print "Next reg is " + str(
self.proj.rfi.regions[next_reg].name.lower())
for i in range(
len(self.proj.rfi.transitions[current_reg][next_reg])):
pointArray_transface = [
x for x in self.proj.rfi.transitions[current_reg]
[next_reg][i]
]
transFace = asarray(
map(self.coordmap_map2lab, pointArray_transface))
bundle_x = (transFace[0, 0] +
transFace[1, 0]) / 2 #mid-point coordinate x
bundle_y = (transFace[0, 1] +
transFace[1, 1]) / 2 #mid-point coordinate y
q_gBundle = hstack((q_gBundle, vstack(
(bundle_x, bundle_y))))
q_gBundle = q_gBundle.transpose()
# Find the closest face to the current position
max_magsq = 1000000
for tf in q_gBundle:
magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
if magsq < max_magsq:
connection = 0
tf = tf + (tf - asarray(self.currentRegionPoly.center(
))) / norm(tf - asarray(self.currentRegionPoly.center(
))) * 2.1 * self.PioneerLengthHalf
if not self.nextRegionPoly.covers(
PolyShapes.Circle(self.PioneerLengthHalf * 2,
(tf[0], tf[1]))):
tf = tf - (tf - asarray(
self.currentRegionPoly.center())) / norm(
tf - asarray(self.currentRegionPoly.center(
))) * 4.2 * self.PioneerLengthHalf
if self.nextRegionPoly.covers(
PolyShapes.Circle(
self.PioneerLengthHalf * 2,
(tf[0], tf[1]))):
connection = 1
else:
connection = 1
if connection == 1:
pt1 = tf
max_magsq = magsq
transFace = 1
self.q_g[0] = pt1[0]
self.q_g[1] = pt1[1]
else:
sample = False
while not sample:
self.q_g[0], self.q_g[
1] = self.nextRegionPoly.sample(
random.random)
robo = PolyShapes.Circle(
self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
if not bool(robo - self.nextRegionPoly):
sample = True
"""
# Push the goal point to somewhere inside the next region to ensure the robot will get there.(CHECK!!)
self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
"""
#plot exiting point
if self.PLOT_EXIT == True:
plt.figure(self.overlap_figure)
plt.clf()
plt.plot(q_gBundle[:, 0], q_gBundle[:, 1], 'ko')
plt.plot(self.q_g[0], self.q_g[1], 'ro')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
if transFace is None:
print "ERROR: Unable to find transition face between regions %s and %s. Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (
self.proj.rfi.regions[current_reg].name,
self.proj.rfi.regions[next_reg].name)
##################################################
#######check whether obstacle is detected#########
##################################################
#Update pose,update self.robot, self.realRobot orientation
self.robot.shift(pose[0] - self.prev_pose[0],
pose[1] - self.prev_pose[1])
self.realRobot.shift(pose[0] - self.prev_pose[0],
pose[1] - self.prev_pose[1])
self.robot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
self.realRobot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
self.prev_pose = pose
############################
########### STEP 1##########
############################
##Check whether obsRange overlaps with obstacle or the boundary (overlap returns the part of robot not covered by the region
# for real Pioneer robot
if self.system == 1:
# motion controller is not in boundary following mode
if self.boundary_following == False:
if self.robocomm.getReceiveObs() == False:
overlap = self.robot - (self.map_work)
else:
overlap = self.robot - (self.map_work -
self.robocomm.getObsPoly())
else: #TRUE
# use a robot with full range all around it
Robot = PolyShapes.Circle(self.obsRange, (pose[0], pose[1]))
Robot.shift(self.shift * cos(pose[2]),
self.shift * sin(pose[2]))
if self.robocomm.getReceiveObs() == False:
overlap = Robot - (self.map_work)
else:
overlap = Robot - (self.map_work -
self.robocomm.getObsPoly())
# for ODE
else:
if self.boundary_following == False:
overlap = self.robot - (self.map_work)
else: #TRUE
overlap = self.robot - (self.map_work)
if self.boundary_following == False:
if bool(overlap): ## overlap of obstacles
#print "There MAYBE overlap~~ check connection to goal"
# check whether the real robot or and path to goal overlap with the obstacle
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - (self.map_work)
else:
pathOverlap = path - (self.map_work -
self.robocomm.getObsPoly())
else:
pathOverlap = path - (self.map_work)
if bool(
pathOverlap
): # there is overlapping, go into bounding following mode
#print "There IS overlap"
self.q_hit = mat([pose[0], pose[1]]).T
self.boundary_following = True
#Generate m-line polygon
QHitPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
(pose[0], pose[1]))
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
(self.q_g[0], self.q_g[1]))
self.m_line = PolyUtils.convexHull(QHitPoly + QGoalPoly)
#plot the first overlap
if self.PLOT_M_LINE == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(QHitPoly, 'k')
self.plotPoly(QGoalPoly, 'k')
self.plotPoly(overlap, 'g')
self.plotPoly(self.m_line, 'b')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs(
) == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
#print "no obstacles 2-ODE true"
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs(
) == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
#print "no obstacles 1-ODE true"
if self.boundary_following == True:
self.q_hit_count += 1
# finding the point to go normal to (closest overlapping point)
j = 0
recheck = 0
while not bool(overlap):
# cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
j += 1
# finding whether the previous obstacle point is on the left side or the right side of the robot
# angle = angle of the previous point from the x-axis of the field
# omega = angle of the current Pioneer orientation from the x-axis of the field
# cc = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
x = self.prev_follow[0] - pose[0]
y = self.prev_follow[1] - pose[1]
angle = atan(y / x)
# convert angle to 2pi
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x < 0 and y < 0:
angle = pi + angle
else:
angle = 2 * pi + angle
# convert pose to 2pi
if pose[2] < 0:
omega = (2 * pi + pose[2])
else:
omega = pose[2]
if omega > angle:
cc = 2 * pi - (omega - angle)
else:
cc = angle - omega
# on the left
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range * 2 * j,
self.range * 2 * j)
Robot.shift(pose[0] - self.range * j * 2,
pose[1] - self.range * j)
Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])
# on the right
else:
#print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range * 2 * j,
self.range * 2 * j)
Robot.shift(pose[0], pose[1] - self.range * j)
Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])
if self.system == 1:
overlap = Robot - (self.map_work -
self.robocomm.getObsPoly())
else:
overlap = Robot - (self.map_work)
#self.plotPoly(Robot, 'm',2)
#determines as dynamic obstacles and can be go striaight to the goal point
if j >= 2:
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
overlap = None
self.overlap = overlap
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
self.drive_handler.setVelocity(vx, vy, pose[2])
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf +
0.06,
(pose[0], pose[1])) ###0.05
departed = not self.currentRegionPoly.overlaps(
self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
return arrived
##extra box plotting in figure 1#
if self.PLOT_OVERLAP == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
self.plotPoly(overlap, 'g', 3)
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.original_figure)
# find the closest point on the obstacle to the robot
overlap_len = len(overlap)
for j in range(overlap_len):
BoundPolyPoints = asarray(overlap[j])
for i in range(len(BoundPolyPoints) - 1):
bundle_x = (
BoundPolyPoints[i, 0] +
BoundPolyPoints[1 + i, 0]) / 2 #mid-point coordinate x
bundle_y = (
BoundPolyPoints[i, 1] +
BoundPolyPoints[1 + i, 1]) / 2 #mid-point coordinate y
q_overlap = hstack((q_overlap, vstack(
(bundle_x, bundle_y))))
bundle_x = (BoundPolyPoints[len(BoundPolyPoints) - 1, 0] +
BoundPolyPoints[0, 0]) / 2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[len(BoundPolyPoints) - 1, 1] +
BoundPolyPoints[0, 1]) / 2 #mid-point coordinate y
q_overlap = hstack((q_overlap, vstack((bundle_x, bundle_y))))
q_overlap = q_overlap.transpose()
pt = self.closest_pt([pose[0], pose[1]],
vstack(
(q_overlap,
asarray(PolyUtils.pointList(overlap)))))
self.prev_follow = pt
#calculate the vector to follow the obstacle
normal = mat([pose[0], pose[1]] - pt)
#find the distance from the closest point
distance = norm(normal)
velocity = normal * self.trans_matrix
vx = (velocity / norm(velocity) / 3)[0, 0]
vy = (velocity / norm(velocity) / 3)[0, 1]
# push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
turn = pi / 4 * (distance - 0.5 * self.obsRange) / (
self.obsRange
) ### change to 0.6 from 0.5 for more allowance in following
corr_matrix = mat([[cos(turn), -sin(turn)], [sin(turn),
cos(turn)]])
v = corr_matrix * mat([[vx], [vy]])
vx = v[0, 0]
vy = v[1, 0]
##plotting overlap on figure 2
if self.PLOT_OVERLAP == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(self.m_line, 'b')
self.plotPoly(overlap, 'r')
plt.plot(pt[0], pt[1], 'ro')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
## conditions that the loop will end
#for 11111
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
(pose[0], pose[1])) ####0.05
departed = not self.currentRegionPoly.overlaps(self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
#for 33333
reachMLine = self.m_line.overlaps(RobotPoly)
# 1.reached the next region
if arrived:
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
print "arriving at the next region. Exit boundary following mode"
vx = 0
vy = 0
"""
# 2.q_hit is reencountered
elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
print "reencounter q_hit. cannot reach q_goal"
vx = 0
vy = 0
"""
# 3.m-line reencountered
elif reachMLine:
#print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
if norm(self.q_g - mat([pose[0], pose[1]]).T
) < norm(self.q_g - self.q_hit) - 2 * self.obsRange:
#print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
#print "leaving boundary following mode"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
leaving = False
# turn the robot till it is facing the goal
while not leaving:
x = self.q_g[0] - self.pose_handler.getPose()[0]
y = self.q_g[1] - self.pose_handler.getPose()[1]
angle = atan(y / x)
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x < 0 and y < 0:
angle = pi + angle
else:
angle = 2 * pi + angle
if self.pose_handler.getPose()[2] < 0:
omega = (2 * pi + self.pose_handler.getPose()[2])
#print >>sys.__stdout__,"omega<0: "+ str(omega)
else:
omega = self.pose_handler.getPose()[2]
#print >>sys.__stdout__,"omega: "+ str(omega)
if omega > angle:
cc = 2 * pi - (omega - angle)
else:
cc = angle - omega
# angle(goal point orientation) on the left of omega(robot orientation)
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print>>sys.__stdout__, "turn left"
vx, vy = self.turnLeft(cc)
# on the right
else:
#print>>sys.__stdout__, "turn right"
vx, vy = self.turnRight(2 * pi - cc)
#print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
self.drive_handler.setVelocity(
vx, vy,
self.pose_handler.getPose()[2])
if omega - angle < pi / 6 and omega - angle > -pi / 6:
leaving = True
#Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - (self.map_work)
else:
pathOverlap = path - (self.map_work -
self.robocomm.getObsPoly())
else:
pathOverlap = path - (self.map_work)
if not bool(pathOverlap):
#print "There is NO MORE obstacles in front for now."
# check if the robot is closer to the goal compared with q_hit
if norm(self.q_hit - mat(self.q_g).T) > norm(
mat([pose[0], pose[1]]).T - mat(self.q_g).T):
#print "The robot is closer than the leaving point. The robot can leave"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
lala = 1
#print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))
"""
# Pass this desired velocity on to the drive handler
# Check if there are obstacles within 0.35m of the robot, if so, stop the robot
if self.system == 1:
if self.robocomm.getSTOP() == True:
vx = 0
vy = 0
"""
#vx = 0
#vy = 0
self.overlap = overlap
self.drive_handler.setVelocity(vx, vy, pose[2])
# Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
self.previous_current_reg = current_reg
# check whether robot has arrived at the next region
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
(pose[0], pose[1])) ###0.05
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
departed = not self.currentRegionPoly.overlaps(self.realRobot)
arrived = self.nextRegionPoly.covers(self.realRobot)
if arrived:
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
if departed and (
not arrived) and (time.time() - self.last_warning) > 0.5:
print "WARNING: Left current region but not in expected destination region"
# Figure out what region we think we stumbled into
for r in self.proj.rfi.regions:
pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
vertices = mat(pointArray).T
if is_inside([pose[0], pose[1]], vertices):
#print "I think I'm in " + r.name
#print pose
break
self.last_warning = time.time()
return arrived
def __init__(self, proj, shared_data, robot_type):
"""
Bug alogorithm motion planning controller
robot_type (int): Which robot is used for execution. pioneer is 1, ODE is 2 (default=1)
"""
self.velocity_count_thres = 100
self.velocity_count = 0
#operate_system (int): Which operating system is used for execution. Ubuntu and Mac is 1, Windows is 2
if sys.platform in ['win32', 'cygwin']:
self.operate_system = 2
else:
self.operate_system = 1
# Information about the robot
## 1: Pioneer ; 2: 0DE
if robot_type not in [1, 2]:
robot_type = 1
self.system = robot_type
#settings for Ubuntu or Mac plotting (only when you set operate_system = 1)
self.PLOT = False # plot with matplot
self.PLOT_M_LINE = False # plot m-line
self.PLOT_EXIT = False # plot exit point of a region
self.PLOT_OVERLAP = False # plot overlap area with the obstacle
#settings for Windows (only when you set operate_system = 2)
self.PLOT_WINDOWS = True
# Get references to handlers we'll need to communicate with
self.drive_handler = proj.h_instance['drive']
self.pose_handler = proj.h_instance['pose']
if self.system == 1:
self.robocomm = shared_data['robocomm']
# Get information about regions
self.proj = proj
self.coordmap_map2lab = proj.coordmap_map2lab
self.coordmap_lab2map = proj.coordmap_lab2map
self.last_warning = 0
###################################
########used by Bug algorithm######
###################################
# PARAMETERS
#for plotting
self.time = time.clock()
self.time_thres = 1
# Pioneer related parameters
self.PioneerWidthHalf = 0.20 #0.25 # (m) width of Pioneer #0.20
self.PioneerLengthHalf = 0.25 #0.30 (m) lenght of Pioneer #0.25
# Real Robot polygon related parameters
self.boxRealVertical = self.PioneerLengthHalf * 2
self.boxRealHorizontal = self.PioneerWidthHalf * 2.5
self.boxRealVertical_shift = self.boxRealVertical / 2
self.boxRealHorizontal_shift = self.boxRealHorizontal / 2
# Pioneer Range related parameters
self.range = 2 * self.PioneerLengthHalf + 0.40 # (m) specify the range of the robot (when the normal circle range cannot detect obstacle) #0.85
self.obsRange = self.range * 0.7 # (m) range that says the robot detects obstacles #0.25
self.shift = 0.20 # How far the range is shifted to ensure it is sensing region in front is bigger 0.20
self.boxVertical = self.obsRange * 2 # box cutting from range of Pioneer
self.boxHorizontal = self.obsRange * 2 # box cutting from range of Pioneer
self.boxVertical_shift = self.boxVertical + self.boxRealVertical / 2 * 1.5 # vertical shifting of box
self.boxHorizontal_shift = self.boxHorizontal / 2 # horizontal shifting of the box
## 2: 0DE
self.factorODE = 30 # 30 works better than 50
if self.system == 2:
self.PioneerWidthHalf = self.PioneerWidthHalf * self.factorODE
self.PioneerLengthHalf = self.PioneerLengthHalf * self.factorODE
self.range = self.range * self.factorODE
self.obsRange = self.obsRange * self.factorODE
self.shift = self.shift * self.factorODE
self.boxVertical = self.boxVertical * self.factorODE
self.boxHorizontal = self.boxHorizontal * self.factorODE
self.boxVertical_shift = self.boxVertical_shift * self.factorODE
self.boxHorizontal_shift = self.boxHorizontal_shift * self.factorODE
self.boxRealVertical = self.boxRealVertical * self.factorODE
self.boxRealHorizontal = self.boxRealHorizontal * self.factorODE
self.boxRealVertical_shift = self.boxRealVertical_shift * self.factorODE
self.boxRealHorizontal_shift = self.boxRealHorizontal_shift * self.factorODE
self.map = {} # dictionary for all the regions
self.all = Polygon.Polygon() # Polygon with all the regions
self.map_work = Polygon.Polygon(
) # Polygon of the current region and next region considered
self.ogr = Polygon.Polygon(
) #Polygon built from occupancy grid data points
self.previous_current_reg = None # previous current region
self.currentRegionPoly = None # current region's polygon
self.nextRegionPoly = None # next region's polygon
self.overlap = None
self.q_g = [0, 0] # goal point of the robot heading to
self.q_hit = [0,
0] # location where the robot first detect an obstacle
self.boundary_following = False # tracking whether it is in boundary following mode
self.m_line = None # m-line polygon
self.trans_matrix = mat(
[[0, 1], [-1, 0]]
) # transformation matrix for find the normal to the vector connecting a point on the obstacle to the robot
self.q_hit_count = 0
self.q_hit_Thres = 1000
self.prev_follow = [[], []]
## Construct robot polygon (for checking overlap)
pose = self.pose_handler.getPose()
self.prev_pose = pose
self.robot = PolyShapes.Rectangle(self.boxHorizontal, self.boxVertical)
self.robot.shift(pose[0] - self.boxHorizontal_shift,
pose[1] - self.boxVertical_shift)
self.robot = PolyShapes.Circle(self.obsRange,
(pose[0], pose[1])) - self.robot
self.robot.rotate(pose[2] - pi / 2, pose[0], pose[1])
self.robot.shift(self.shift * cos(pose[2]), self.shift * sin(pose[2]))
#construct real robot polygon( see if there is overlaping with path to goal
self.realRobot = PolyShapes.Rectangle(self.boxRealHorizontal,
self.boxRealVertical)
self.realRobot.shift(pose[0] - self.boxRealHorizontal_shift,
pose[1] - self.boxRealVertical_shift)
self.realRobot.rotate(pose[2] - pi / 2, pose[0], pose[1])
#constructing polygon of different regions (holes being taken care)
for region in self.proj.rfi.regions:
self.map[region.name] = self.createRegionPolygon(region)
for n in range(len(region.holeList)): # no of holes
self.map[region.name] -= self.createRegionPolygon(region, n)
#construct a polygon that included all the regions
for regionName, regionPoly in self.map.iteritems():
self.all += regionPoly
#setting for plotting
if self.operate_system == 1:
if self.PLOT or self.PLOT_OVERLAP == True:
self.original_figure = 1
plt.figure(self.original_figure)
if self.PLOT_EXIT or self.PLOT_M_LINE == True:
self.overlap_figure = 2
plt.figure(self.overlap_figure)
else:
if self.PLOT_WINDOWS == True:
# start using anmination to plot Pioneer
self.fig = plt.figure()
self.ax = self.fig.add_subplot(111)
self.scope = _Scope(self.ax, self)
thread.start_new_thread(self.jplot, ())
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.original_figure)
def processData(self, data):
"""
Save the data to the appropriate category for possible extraction.
"""
#print 'I GOT DATA',data,[0],data[1]
# Check for valid data (not null or empty string)
#print '**************NOTIFICATION***************',type(_RobotCommunicator.WALL_HEADER),type(data[0])
if data:
#print '**************NOTIFICATION***************',type(_RobotCommunicator.WALL_HEADER),type(data[0]),_RobotCommunicator.WALL_HEADER==data[0]
# Check header and assign data appropriately
# TODO: Check length of data for validity
#print 'Header',data[0]
if data[0] == _RobotCommunicator.POSE_HEADER:
self.pose = unpack(_RobotCommunicator.POSE_FORMAT, data[1:])
elif data[0] == _RobotCommunicator.SENSOR_HEADER:
#for i in range(1, len(data)-1, 2):
index = unpack('B', data[1])
value = unpack('?', data[2])
# Update old values or create new sensor-value pair
self.sensors[index[0]] = value[0]
#print 'in csharp: ',[index,value]
elif data[0] == _RobotCommunicator.WAYPOINT_HEADER:
self.waypoints = [] # Clear old waypoints
for i in range(1, len(data) - 16, 16):
x, y = unpack(_RobotCommunicator.WAYPOINT_FORMAT,
data[i:i + 15])
self.waypoints.append((x, y))
elif data[0] == _RobotCommunicator.DIRECTION_HEADER:
self.direction = unpack(_RobotCommunicator.DIRECTION_FORMAT,
data[1:])
elif data[0] == _RobotCommunicator.ACTUATOR_HEADER:
self.actuators = [
] # Clear old actuator commands for i in range(1, len(data)-1):
self.actuators.append(
unpack(_RobotCommunicator.ACTUATOR_FORMAT, data[i]))
elif data[0] == _RobotCommunicator.WALL_HEADER:
self.walls = {} # Clear old wall entries
index = unpack('B', data[1])
x1, y1, x2, y2 = unpack(_RobotCommunicator.WALL_FORMAT,
data[2:34])
self.walls = (x1, y1, x2, y2)
#print '**************Coordinates***************',(x1,y1,x2,y2)
print '****self.walls*********', self.walls
elif data[0] == _RobotCommunicator.OBS_HEADER:
index = unpack('B', data[1])
add, x1, y1 = unpack(_RobotCommunicator.OBS_FORMAT, data[2:26])
#print '***********self.obs*************'+','.join(map(str,[add,x1,y1]))
self.obs = [add, x1, round(y1, 2)]
if add == 1:
a = PolyShapes.Rectangle(self.resolX, self.resolY)
a.shift(x1, y1)
self.obsPoly += a
self.receiveObs = True
#print "add obstacle:" + str(x1) + ","+ str(y1)
elif add == 4:
if x1 == 0:
self.STOP = True
else:
self.STOP = False
else:
a = PolyShapes.Rectangle(self.resolX, self.resolY)
a.shift(x1, y1)
self.obsPoly -= a
self.receiveObs = True
#print "del obstacle:"+ str(x1) + ","+ str(y1)
else:
print "Unexpected or corrupted data packet received."