class TestPoint(ClusterCandidate): def __init__(self, x, y): ClusterCandidate.__init__(self) self.point = Point(x, y) def distance_to_candidate(self, other_candidate): return self.point.distance_to(other_candidate.point)
class BugBrain: #Tolerance for distance between points POINT_TOLERANCE = 0.5 #Tolerance for line detection TOLERANCE = 0.3 def __init__(self, goal_x, goal_y, side): self.wp_goal_point = Point(goal_x, goal_y) self.first_line = True self.goal_unreachable = True self.pose_list = [] self.old_is_left = True self.new_is_left = True pass def follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ #Create entry pose when finding wall self.wp_entry_pose = Point(x, y) #Create line only on first obstacle encountered if self.first_line: self.ln_goal_vector = Line.from_points( [self.wp_goal_point, self.wp_entry_pose]) self.first_line = False self.old_is_left = self.ln_goal_vector.point_left(self.wp_entry_pose) pass def leave_wall(self, x, y, theta): """ This function is called when the state machine leaves the wallfollower state. """ pass def is_goal_unreachable(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether the goal is unreachable. """ if self.is_pose_repeated(Point(x, y)) > 1: return True return False def is_time_to_leave_wall(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether it is the right time (or place) to leave the wall and move straight to the goal. """ self.wp_test_pose = Point(x, y) self.new_is_left = self.ln_goal_vector.point_left(self.wp_test_pose) #Line for comparing if robot is to left self.ln_robot = Line.from_points( [Point(x, y), Point(x + cos(theta), y + sin(theta))]) #Check for ditance to entry point if abs(self.wp_test_pose.distance_to( self.wp_entry_pose)) > self.POINT_TOLERANCE: #Check for a change on side if self.old_is_left != self.new_is_left: self.old_is_left = self.new_is_left #Save point self.pose_list.append(self.wp_test_pose) #Check goal is left to robot if self.ln_robot.distance_to(self.wp_goal_point) < 0: #Check if first time on point, then leave if self.is_pose_repeated(self.wp_test_pose) == 1: return True return False def is_pose_repeated(self, pose): """ This functions returns the times the robot has gone through point pose """ counter = 0 for member in self.pose_list: if abs(pose.distance_to(member)) < self.POINT_TOLERANCE: counter = counter + 1 return counter
class BugBrain: TOLERANCE = 0.3 def __init__(self, goal_x, goal_y, side): self.goal_x = goal_x self.goal_y = goal_y self.side = side self.hit_points_list = [] self.Vec_hit = None self.angle_vec = None self.last_hit_dist = None self.current_to_leave_dist = None self.unreach_points = None self.list_leaving_points = [] self.x1 = None self.y1 = None self.leave_wall_list = [] self.wp_leave_wall_points = None self.wp_goal_point = Point(self.goal_x, self.goal_y) self.count = 0 self.flag = False def follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ self.x = x self.y = y self.theta = theta self.wp_hit_point = Point(self.x, self.y) #creating a list for all the points it hits self.hit_points_list.append((x,y)) self.ln_line_to_goal = Line.from_points([self.wp_hit_point, self.wp_goal_point]) # Draws a line from hit point to goal point self.Vec_hit = Point(x,y) self.angle_vec = self.wp_goal_point-self.Vec_hit print self.angle_vec self.last_hit_dist = self.Vec_hit.distance_to(self.wp_goal_point) self.count = self.count + 1 def leave_wall(self, x, y, theta): """ This function is called when the state machine leaves the wallfollower state. """ self.flag = True self.wp_leave_wall_points = Point(x,y) def is_goal_unreachable(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether the goal is unreachable. """ # self.x_unreach = x # self.y_unreach = y # self.wp_current_pos = Point(x,y) # self.wp_hit_point.distance_to(self.wp_current_pos) # print self.wp_current_pos # if (self.flag == False): # if(abs(self.wp_hit_point.distance_to(self.wp_current_pos))-abs(self.wp_goal_point.distance_to(self.wp_current_pos)) < 0.3): # return True # return False def is_time_to_leave_wall(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether it is the right time (or place) to leave the wall and move straight to the goal. """ self.x1 = x self.y1 = y self.theta = theta v1 = Vec2(x,y) goal_vec = self.wp_goal_point v3 = v1 - goal_vec #print v3 angles_diff = v3.angle_to(self.angle_vec) #print angles_diff self.current_to_leave_dist = v1.distance_to(goal_vec) if (abs(self.x - self.x1)>0.5 or abs(self.y - self.y1)>0.5) and (abs(angles_diff)<2 or (abs(angles_diff)<182 and abs(angles_diff)>178 )): self.list_leaving_points.append((self.x1,self.y1)) if self.current_to_leave_dist < self.last_hit_dist: return True return False
class BugBrain: # declearing constants def LEFT_WALLFOLLOWING(self): return 0; def RIGHT_WALLFOLLOWING(self): return 1; def LEFT_SIDE(self): return -1; def RIGHT_SIDE(self): return 1; def TOLERANCE(self): return planar.EPSILON; def __init__(self, goal_x, goal_y, side): self.wall_side = side; self.wp_destination = Point(goal_x, goal_y); self.path_started = False; #the tolerence == planar.EPSILON at default value does not work good planar.set_epsilon(0.1); self.distance_when_left = 9999; # method to determin if the destenation is on opposit side of wall being followed. # @param: distance # signed distance from the robot to goal. # can be obtained by path_line.distance_to(ROBOT CURRENT POSITION). def is_destination_opposite_to_wall(self,distance): direction = math.copysign(1,distance); if(self.wall_side == self.LEFT_WALLFOLLOWING()): if(direction == self.RIGHT_SIDE()): return True; else: return False; else: if(direction == self.LEFT_SIDE()): return True; else: return False; def follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ # compute and store necessary variables theta = degrees(theta); position = Point(x,y); self.ln_path = Line.from_points([position,self.wp_destination]); # saving where it started wall following self.wp_wf_start = position; def leave_wall(self, x, y, theta): """ This function is called when the state machine leaves the wallfollower state. """ # compute and store necessary variables self.path_started = False; self.distance_when_left = self.wp_destination.distance_to(Point(x,y)); self.wp_left_wall_at = Point(x,y); # self.wp_when_left = pass def is_goal_unreachable(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether the goal is unreachable. """ # if the robot goes around an obstacle and # reaches the starting point and the destenation is still not reached then # the goal is unreachable. distance_to_path= self.ln_path.distance_to(Point(x,y)); if(abs(distance_to_path) < self.TOLERANCE() and Vec2(x,y).almost_equals(self.wp_wf_start) and self.path_started): # self.path_started = False; rospy.logwarn("UNREACHABLE POINT!"); return True return False def is_time_to_leave_wall(self, x, y, theta): """ This function is regularly called from the wallfollower state to check the brain's belief about whether it is the right time (or place) to leave the wall and move straight to the goal. """ theta = degrees(theta); self.current_theta =theta; self.wp_current_position = Point(x,y); self.current_direction = Vec2.polar(angle = theta,length = 1); #Robot Orientation Line. self.ln_current_orentation = Line(Vec2(x,y),self.current_direction); # the prependicular line to the path self.ln_distance = self.ln_path.perpendicular(self.wp_current_position); distance_to_path= self.ln_path.distance_to(Point(x,y)); self.distance_to_path = distance_to_path; distance_to_destination = self.ln_current_orentation.distance_to(self.wp_destination); if(abs(distance_to_path) > 1): self.path_started =True; self.distance_to_goal = self.wp_destination.distance_to(Point(x,y)); """ checking if distance to the straight path is approx. 0 and if destenation on the opposit side of wall then leave the path NOTE and TODO: works only for the circles not for complex path. """ if(abs(distance_to_path) < self.TOLERANCE() and self.distance_to_goal < self.distance_when_left and self.is_destination_opposite_to_wall(distance_to_destination) and self.path_started): # is robot started following wall! self.wp_wf_stop = Point(x,y); return True; return False