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
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
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
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
