def gotoRegion(self, current_reg, next_reg, last=False):
"""
If ``last`` is true, we will move to the center of the region.
Returns ``True`` if we are outside the supposed ``current_reg``
"""
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
controller = self.get_controller(current_reg, next_reg, last)
pose = self.pose_handler.getPose()
[X, DqX, F, inside, J] = controller(mat(pose[0:2]).T)
self.drive_handler.setVelocity(X[0,0], X[1,0], pose[2])
# Transform the region vertices into real coordinates
pointArray = [self.fwd_coordmap(x) for x in self.rfi.regions[next_reg].getPoints()]
vertices = mat(pointArray).T
# Figure out whether we've reached the destination region
if is_inside([pose[0], pose[1]], vertices):
arrived = True
else:
arrived = False
if (arrived != (not inside)) 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.rfi.regions:
pointArray = [self.fwd_coordmap(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 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.
"""
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Find our current configuration
pose = self.pose_handler.getPose()
# Check if Vicon has cut out
# TODO: this should probably go in posehandler?
if math.isnan(pose[2]):
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 tree will be used.
if not self.previous_next_reg == current_reg:
self.nextRegionPoly = self.map[self.proj.rfi.regions[next_reg].name]
self.currentRegionPoly = self.map[self.proj.rfi.regions[current_reg].name]
print "next Region is "+str(self.proj.rfi.regions[next_reg].name)
print "Current Region is "+str(self.proj.rfi.regions[current_reg].name)
pointArray = [x for x in self.proj.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
vertices = mat(pointArray).T
#set to zero velocity before tree is generated
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
# Find a face to go through
# TODO: Account for non-determinacy?
transFace = None
# Find the index of this face
q_gBundle = [[],[]]
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))))
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)
# Run algorithm to build the Rapid-Exploring Random Trees
self.RRT_V = None
self.RRT_E = None
self.RRT_V,self.RRT_E,self.heading,self.E_prev,self.RRT_V_toPass,self.RRT_E_toPass = _RRTControllerHelper.buildTree([pose[0], pose[1]],pose[2], vertices, self.radius,self.system,self.currentRegionPoly,self.nextRegionPoly,q_gBundle,self.map,self.all)
# map the lab coordinates back to pixels
V_tosend = array(mat(self.RRT_V[1:,:])).T
V_tosend = map(self.coordmap_lab2map, V_tosend)
V_tosend = mat(V_tosend).T
s = 'RRT:E'+"["+str(list(self.RRT_E[0]))+","+str(list(self.RRT_E[1]))+"]"+':V'+"["+str(list(self.RRT_V[0]))+","+str(list(V_tosend[0]))+","+str(list(V_tosend[1]))+"]"+':T'+"["+str(list(q_gBundle[0]))+","+str(list(q_gBundle[1]))+"]"
print s
# Run algorithm to find a velocity vector (global frame) to take the robot to the next region
V = _RRTControllerHelper.setVelocity([pose[0], pose[1]], self.RRT_V,self.RRT_E,self.heading,self.E_prev,self.radius)
self.Velocity = V[0:2,0]
self.heading = V[2,0]
self.E_prev = V[3,0]
self.previous_next_reg = current_reg
# Pass this desired velocity on to the drive handler
self.drive_handler.setVelocity(V[0,0], V[1,0], pose[2])
RobotPoly = Polygon.Shapes.Circle(self.radius,(pose[0],pose[1]))
#step 4: check whether robot is inside the boundary
departed = not self.currentRegionPoly.covers(RobotPoly)
arrived = self.nextRegionPoly.covers(RobotPoly)
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 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.
"""
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Find our current configuration
pose = self.pose_handler.getPose()
# Check if Vicon has cut out
# TODO: this should probably go in posehandler?
if math.isnan(pose[2]):
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 tree will be used.
if not self.previous_next_reg == current_reg:
self.nextRegionPoly = self.map[
self.proj.rfi.regions[next_reg].name]
self.currentRegionPoly = self.map[
self.proj.rfi.regions[current_reg].name]
print "next Region is " + str(self.proj.rfi.regions[next_reg].name)
print "Current Region is " + str(
self.proj.rfi.regions[current_reg].name)
pointArray = [
x for x in self.proj.rfi.regions[current_reg].getPoints()
]
pointArray = map(self.coordmap_map2lab, pointArray)
vertices = mat(pointArray).T
#set to zero velocity before tree is generated
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
# Find a face to go through
# TODO: Account for non-determinacy?
transFace = None
# Find the index of this face
q_gBundle = [[], []]
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))))
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)
# Run algorithm to build the Rapid-Exploring Random Trees
self.RRT_V = None
self.RRT_E = None
self.RRT_V, self.RRT_E, self.heading, self.E_prev, self.RRT_V_toPass, self.RRT_E_toPass = _RRTControllerHelper.buildTree(
[pose[0], pose[1]], pose[2], vertices, self.radius,
self.system, self.currentRegionPoly, self.nextRegionPoly,
q_gBundle, self.map, self.all)
# map the lab coordinates back to pixels
V_tosend = array(mat(self.RRT_V[1:, :])).T
V_tosend = map(self.coordmap_lab2map, V_tosend)
V_tosend = mat(V_tosend).T
s = 'RRT:E' + "[" + str(list(self.RRT_E[0])) + "," + str(
list(self.RRT_E[1])) + "]" + ':V' + "[" + str(
list(self.RRT_V[0])) + "," + str(list(
V_tosend[0])) + "," + str(list(
V_tosend[1])) + "]" + ':T' + "[" + str(
list(q_gBundle[0])) + "," + str(
list(q_gBundle[1])) + "]"
print s
# Run algorithm to find a velocity vector (global frame) to take the robot to the next region
V = _RRTControllerHelper.setVelocity([pose[0], pose[1]], self.RRT_V,
self.RRT_E, self.heading,
self.E_prev, self.radius)
self.Velocity = V[0:2, 0]
self.heading = V[2, 0]
self.E_prev = V[3, 0]
self.previous_next_reg = current_reg
# Pass this desired velocity on to the drive handler
self.drive_handler.setVelocity(V[0, 0], V[1, 0], pose[2])
RobotPoly = Polygon.Shapes.Circle(self.radius, (pose[0], pose[1]))
#step 4: check whether robot is inside the boundary
departed = not self.currentRegionPoly.covers(RobotPoly)
arrived = self.nextRegionPoly.covers(RobotPoly)
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 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 "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
self.map_work = Polygon(self.map)
# create polygon list for regions other than the current_reg and the next_reg. This polygon will be counted as the obstacle if encountered.
if not self.rfi.regions[current_reg].name.lower() == 'freespace':
pointArray = [x for x in self.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map_work -= Polygon(regionPoints)
if not self.rfi.regions[next_reg].name.lower() == 'freespace':
pointArray = [x for x in self.rfi.regions[next_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.map_work -= Polygon(regionPoints)
# NOTE: Information about region geometry can be found in self.rfi.regions
# building current polygon and destination polygon
pointArray = [x for x in self.rfi.regions[next_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.nextRegionPoly = Polygon(regionPoints)
if self.rfi.regions[next_reg].name.lower()=='freespace':
for region in self.rfi.regions:
if region.name.lower() != 'freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.nextRegionPoly = self.nextRegionPoly - Polygon(regionPoints)
pointArray = [x for x in self.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.currentRegionPoly = Polygon(regionPoints)
if self.rfi.regions[current_reg].name.lower()=='freespace':
for region in self.rfi.regions:
if region.name.lower() != 'freespace':
pointArray = [x for x in region.getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
regionPoints = [(pt[0],pt[1]) for pt in pointArray]
self.currentRegionPoly = self.currentRegionPoly - Polygon(regionPoints)
#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.rfi.regions[current_reg].name.lower())
print "Next reg is "+ str(self.rfi.regions[next_reg].name.lower())
for i in range(len(self.rfi.transitions[current_reg][next_reg])):
pointArray_transface = [x for x in self.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:
pt1 = tf
max_magsq = magsq
transFace = 1
self.q_g[0] = pt1[0]
self.q_g[1] = pt1[1]
# Push the goal point to somewhere inside the next region to ensure the robot will go there.
if self.rfi.regions[current_reg].name.lower()=='freespace':
self.q_g = self.q_g+(self.q_g-asarray(self.nextRegionPoly.center()))/norm(self.q_g-asarray(self.nextRegionPoly.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.nextRegionPoly.center()))/norm(self.q_g-asarray(self.nextRegionPoly.center()))*6*self.PioneerLengthHalf
else:
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 self.currentRegionPoly.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,1)
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.rfi.regions[current_reg].name, self.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
# for real Pioneer robot
if self.system == 1:
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 = 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 = 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= Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = 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 = Circle(self.PioneerLengthHalf/4,(pose[0],pose[1]))
QGoalPoly= Circle(self.PioneerLengthHalf/4,(self.q_g[0],self.q_g[1]))
self.m_line = 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,1)
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)
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 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 = 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 = 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)
##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,0)
# 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(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) ### 5
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(vx,vy,'ko')
plt.plot(pt[0],pt[1],'ro')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,1)
## conditions that the loop will end
#for 11111
RobotPoly = Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ####0.05
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.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.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= Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = 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.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:
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
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 = Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ###0.05
departed = not self.currentRegionPoly.covers(RobotPoly)
arrived = self.nextRegionPoly.covers(self.realRobot)
if arrived:
self.q_hit_count = 0
self.boundary_following = False
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.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 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.
"""
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Find our current configuration
pose = self.pose_handler.getPose()
# Check if Vicon has cut out
# TODO: this should probably go in posehandler?
if math.isnan(pose[2]):
print "WARNING: No Vicon data! Pausing."
self.drive_handler.setVelocity(0, 0) # So let's stop
time.sleep(1)
return False
# NOTE: Information about region geometry can be found in self.rfi.regions:
pointArray = [x for x in self.rfi.regions[current_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
vertices = mat(pointArray).T
if last:
transFace = None
else:
# Find a face to go through
# TODO: Account for non-determinacy?
# For now, let's just choose the largest face available, because we are probably using a big clunky robot
max_magsq = 0
for tf in self.rfi.transitions[current_reg][next_reg]:
magsq = (tf[0].x - tf[1].x)**2 + (tf[0].y - tf[1].y)**2
if magsq > max_magsq:
pt1, pt2 = tf
max_magsq = magsq
transFace = None
# Find the index of this face
# TODO: Why don't we just store this as the index?
for i, face in enumerate([x for x in self.rfi.regions[current_reg].getFaces()]):
# Account for both face orientations...
if (pt1 == face[0] and pt2 == face[1]) or (pt1 == face[1] and pt2 == face[0]):
transFace = i
break
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.rfi.regions[current_reg].name, self.rfi.regions[next_reg].name)
# Run algorithm to find a velocity vector (global frame) to take the robot to the next region
V = vectorControllerHelper.getController([pose[0], pose[1]], vertices, transFace)
# Pass this desired velocity on to the drive handler
self.drive_handler.setVelocity(V[0], V[1], pose[2])
departed = not is_inside([pose[0], pose[1]], vertices)
pointArray = [x for x in self.rfi.regions[next_reg].getPoints()]
pointArray = map(self.coordmap_map2lab, pointArray)
vertices = mat(pointArray).T
# Figure out whether we've reached the destination region
arrived = is_inside([pose[0], pose[1]], vertices)
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.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
from is_inside import *
test = is_inside([100, 200], [100, 200, 300, 400])
if test == True:
print("Done")
else:
print("Do it again")
