def follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ self.wp_start_wall_point = Vec2(x, y) flag = True """ Check if current wall following start point is already inserted into list "self.begin_point_list". If not, append to list """ for x in range(len(self.begin_point_list)): wp_point = self.begin_point_list[x][0] if abs(wp_point.distance_to(self.wp_start_wall_point) <= 1): flag = False if (flag == True): rospy.loginfo("Inserting Begining point to list") self.begin_point_list.append((self.wp_start_wall_point, 0)) rospy.loginfo("Start Wall Follow.") self.now = rospy.get_rostime() """ If this is first hit on any wall, estimate straight line to goal, use this straight line for checking position later """ if len(self.leave_point_list) < 1: rospy.loginfo(" Plot Line...........") self.ln_goal_line = Line.from_points( [self.wp_start_wall_point, self.wp_goal_point]) pass
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 follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ #checking whetre new point is already in the list if not add it self.wp_obstacle_start = Vec2(x, y) is_following = True for x in range(len(self.obstacle_starting_points)): wp_new_point = self.obstacle_starting_points[x][0] if abs(wp_new_point.distance_to(self.wp_obstacle_start) <= 1): is_following = False if (is_following == True): self.obstacle_starting_points.append((self.wp_obstacle_start, 0)) self.time = rospy.get_rostime() #line for checking the position if len(self.obstacle_ending_points) < 1: self.ln_line_to_goal = Line.from_points( [self.wp_obstacle_start, self.wp_goal]) pass
def follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ self.wp_one = (x, y) self.wp_two = (self.goal_x, self.goal_y) self.ln_one = Line.from_points([self.wp_one, self.wp_two]) # compute and store necessary variables pass
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 follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ self.wp_wallfollower_enter = (x, y) # Point of entry of wallfollower mode stored self.ln_goal = Line.from_points([self.wp_goal, (x,y)]) self.ln_goal_perpendicular = self.ln_goal.perpendicular(self.wp_goal) self.ln_enter_wall_perpendicular = self.ln_goal.perpendicular((x,y)) # Line constructed, through points of entering wallfollowr mode and goal. # Perpendiculars to it constructed in this point and in goal point. # They would be used to define robots orientetion relative to goal. self.goal_unreachable = 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 follow_wall(self, x, y, theta): """ This function is called when the state machine enters the wallfollower state. """ self.x1 = x self.y1 = y #adding the current values of x and y in the list self.x_list.append(x) self.y_list.append(y) #creating point for current x,y values self.wp_current_point = Point(x, y) #creating line between current point to goal point self.ln_one = Line.from_points( [self.wp_goal_position, self.wp_current_point]) 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
for x in range(len(self.begin_point_list)): wp_point=self.begin_point_list[x][0] if abs(wp_point.distance_to( self.wp_start_wall_point)<=1): flag=False if (flag==True) : rospy.loginfo("Adding first point to the list") self.begin_point_list.append((self.wp_start_wall_point,0)) rospy.loginfo("Initialize Wall Follow.") self.now = rospy.get_rostime() if len(self.leave_point_list)<1: rospy.loginfo(" Line plot") self.ln_goal_line=Line.from_points([self.wp_start_wall_point, self.wp_goal_point]) pass def leave_wall(self, x, y, theta): """ This function is called when the state machine leaves the wallfollower state. """ #Storing points self.wp_left_wall_point=Vec2(x,y) pass def is_goal_unreachable(self, x, y, theta): """ This function is regularly called from the wallfollower state to check