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 True 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 inRegion_old(self, regionName, initial=False): """ Check if the robot is in this region regionName (string): Name of the region """ if initial: self.prev_pose = self.executor.hsub.getPose() return True else: pose = self.executor.hsub.getPose() ######################################### ### Copied from vectorController.py ##### ######################################### # make sure the pose is valid if sum(pose) == 0: pose = self.prev_pose # maybe do interpolation later? ltlmop_logger.warning("Losing pose... Using old one.") else: self.prev_pose = pose regionNo = self.proj.rfiold.indexOfRegionWithName(regionName) pointArray = [ x for x in self.proj.rfiold.regions[regionNo].getPoints() ] pointArray = map(self.executor.hsub.coordmap_map2lab, pointArray) vertices = numpy.mat(pointArray).T #ltlmop_logger.debug('self.proj.rfiold.regions:' + str(self.proj.rfiold.regions)) #ltlmop_logger.debug("pose:" + str(pose)) #ltlmop_logger.debug("vertices:" + str(vertices)) #ltlmop_logger.debug(self.proj.rfiold.regions[regionNo].name +": " + str(is_inside([pose[0], pose[1]], vertices))) return is_inside([pose[0], pose[1]], vertices)
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. """ 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 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