def rosloop(self):
wall_follower = WallFollower()
wall_follower.init(1.0, 1.0)
twist = Twist()
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if self.state == "TAKE_OFF":
twist.linear.z = 0.1
if self.altitude > 0.5:
self.state = self.transition("WALL_FOLLOWING")
if self.state == "WALL_FOLLOWING":
twist = wall_follower.wall_follower(self.front_range,
self.right_range,
self.current_heading)
pub.publish(twist)
rate.sleep()
class GradientBugController:
#First run of the algorithm
first_run = True
#For wall follower
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
already_reversed_direction = False
direction = 1
#For state transitions
state_start_time = 0
state = "ROTATE_TO_GOAL"
state_WF = ""
#Max values
max_speed = 0.2
max_rate = 0.5
# previous values
heading_prev = 0.0
prev_distance = 1000.0
#For RSSI measurements
do_circle = True
angle_rssi = 0
rssi_array = []
rssi_heading_array = []
rssi_goal_angle_adjust = 0
rssi_linear_array = []
rssi_linear_array_max_size = 49
prev_rssi = 0
t = 0
saved_pose = PoseStamped()
saved_pose_hit = PoseStamped()
overwrite_and_reverse_direction = False
not_out_of_the_woods = True
angle_goal = 0
goal_angle_dir = -1
correct_heading_array = []
rssi_sample_reset = False
prev_rssi = 0
saved_pose_sample = PoseStamped()
def init(self,
new_ref_distance_from_wall,
max_speed_ref=0.2,
max_rate_ref=0.5,
angle_goal_init=-2.27):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.state = "ROTATE_TO_GOAL"
self.max_speed = max_speed_ref
self.max_rate = max_rate_ref
self.rssi_goal_angle_adjust = 0
self.first_run = True
self.do_circle = True
self.rssi_array = []
self.rssi_heading_array = []
self.rssi_linear_array = []
self.state_start_time = 0
self.angle_rssi = 0
self.heading_prev = 0
self.prev_distance = 1000
self.already_reversed_direction = False
self.direction = 1
self.saved_pose = PoseStamped()
self.saved_pose_hit = PoseStamped()
self.overwrite_and_reverse_direction = False
self.not_out_of_the_woods = True
self.loop_angle = 0
self.angle_goal = angle_goal_init
self.goal_angle_dir = -1
self.correct_heading_array = [0, 0, 0, 0, 0, 0, 0, 0]
self.rssi_sample_reset = False
def take_off(self):
twist = Twist()
twist.linear.z = 0.1
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurn(self, rate):
v = 0
w = rate
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurnCircle(self, radius):
v = self.max_speed
w = self.direction * (-v / radius)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistForwardAlongWall(self, range):
twist = Twist()
twist.linear.x = self.max_speed
if self.logicIsCloseTo(self.ref_distance_from_wall, range,
0.1) == False:
if range > self.ref_distance_from_wall:
twist.linear.y = self.direction * (-self.max_speed / 3)
else:
twist.linear.y = self.direction * self.max_speed / 3
return twist
def logicIsCloseTo(self, real_value=0.0, checked_value=0.0, margin=0.05):
if real_value > checked_value - margin and real_value < checked_value + margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = self.simulator_time
return state
def fillHeadingArray(self, rssi_heading, diff_rssi, max_meters):
#heading array of action choices
heading_array = [-135.0, -90.0, -45.0, 0.0, 45.0, 90.0, 135.0, 180.0]
rssi_heading_deg = math.degrees(rssi_heading)
#print(rssi_heading_deg)
for it in range(8):
#fill array based on heading and rssi heading
if ((rssi_heading_deg>=heading_array[it]-22.5) and (rssi_heading_deg<heading_array[it]+22.5) and it is not 7) or \
(it is 7 and ((rssi_heading_deg>=heading_array[it]-22.5) or (rssi_heading_deg<-135.0 - 22.5))):
temp_value_forward = self.correct_heading_array[it]
temp_value_backward = self.correct_heading_array[(it + 4) % 8]
#If gradient is good, increment the array corresponding to the current heading and
# decrement the exact opposite
if (diff_rssi > 0):
self.correct_heading_array[it] = temp_value_forward + 1
if (temp_value_backward > 0):
self.correct_heading_array[(it + 4) %
8] = temp_value_backward - 1
# if gradient is bad, decrement the array corresponding to the current heading and
# increment the exact opposite
elif diff_rssi < 0:
self.correct_heading_array[(it + 4) %
8] = temp_value_backward + 1
if (temp_value_forward > 0):
self.correct_heading_array[it] = temp_value_forward - 1
# degrading function
# If one of the arrays goes over maximum amount of points (meters), then decrement all values
if max(self.correct_heading_array) > max_meters:
for it in range(8):
if self.correct_heading_array[it] > 0:
self.correct_heading_array[
it] = self.correct_heading_array[it] - 1
# Calculate heading where the beacon might be
count = 0
y_part = 0
x_part = 0
for it in range(8):
if (self.correct_heading_array[it] > 0):
x_part += self.correct_heading_array[it] * math.cos(
heading_array[it] * math.pi / 180.0)
y_part += self.correct_heading_array[it] * math.sin(
heading_array[it] * math.pi / 180.0)
count = count + self.correct_heading_array[it]
wanted_angle_return = 0
if count is not 0:
wanted_angle_return = math.atan2(y_part / count, x_part / count)
'''
print("heading", rssi_heading)
print("headingarray", heading_array)
print("rssi diff", diff_rssi)
print ' '
print " "+ str(self.correct_heading_array[7]) + " "
print " " + str(self.correct_heading_array[0]) + " " + str(self.correct_heading_array[6])
print " " + str(self.correct_heading_array[1]) + " " + str(self.correct_heading_array[5])
print " " + str(self.correct_heading_array[2]) + " " + str(self.correct_heading_array[4])
print " "+ str(self.correct_heading_array[3]) + " "
print ' '
print("wanted heading",wanted_angle_return)'''
return wanted_angle_return
def stateMachine(self,
front_range,
right_range,
left_range,
current_heading,
distance_goal,
rssi_to_tower,
odometry,
correct_time,
from_gazebo=True,
WF_argos=None,
RRT=None,
outbound=False,
distance_other_bot=5,
priority=False,
goal_angle_other=0):
#Initialization of the twist command
twist = Twist()
#Save simulator time as the correct time
self.simulator_time = correct_time
# Deal with none values of the range sensors
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
if left_range == None:
left_range = inf
# First thing to take care of at the very first run
if self.first_run is True:
self.prev_distance = 2000
#distance_goal;
self.angle_rssi = 2000
self.first_run = False
self.heading_prev = 0 #current_heading
self.state_start_time = correct_time
bot_is_close = False
bot_is_really_close = False
if distance_other_bot < 2:
bot_is_close = True
if distance_other_bot < 1:
bot_is_really_close = True
# Bearing to goal is the angle to goal
#TODO check if this is also correct for the gazebo implementation....
#################### STATE DEFINITIONS ####################
# FORWARD
# WALLFOLLOWING
# ROTATE_TO_GOAL
# MOVE_OUT_OF_WAY
#print self.overwrite_and_reverse_direction
#################### STATE TRANSITIONS#####################
# Forward
if self.state == "FORWARD":
#If front range is activated to be close et the other wall
if front_range < self.ref_distance_from_wall + 0.2:
#Initialize the wallfollower
if from_gazebo:
self.wall_follower.init(self.ref_distance_from_wall,
self.max_speed)
else:
WF_argos.init()
# save previous heading and initialize the reverse option
self.heading_prev = deepcopy(current_heading)
# First check if the bug has not gotten himself in a loop
if self.overwrite_and_reverse_direction:
self.direction = -1 * self.direction
self.overwrite_and_reverse_direction = False
else:
# To evaluate the wall angle for the local direction (replace scan_obstacle)
rand_num = random.randint(1, 101)
if left_range < right_range and left_range < 2.0:
self.direction = -1
elif left_range > right_range and right_range < 2.0:
self.direction = 1
elif left_range > 2.0 and right_range > 2.0:
self.direction = 1
else:
self.direction = -1
#Save the hitpoint for loop checking
self.saved_pose_hit = deepcopy(odometry)
# GRADIENTBUG: implement reinitialize heading array!!
self.correct_heading_array = [0, 0, 0, 0, 0, 0, 0, 0]
#Go to wall_following
self.state = self.transition("WALL_FOLLOWING")
self.already_reversed_direction = False
pass
# Wall Following
elif self.state == "WALL_FOLLOWING":
#Detect the relative position of the robot and the hitpoint to dietect loops
rel_x_loop = odometry.pose.position.x - self.saved_pose_hit.pose.position.x
rel_y_loop = odometry.pose.position.y - self.saved_pose_hit.pose.position.y
temp_loop_angle = wraptopi(np.arctan2(rel_y_loop, rel_x_loop))
temp_loop_distance = np.sqrt(rel_x_loop * rel_x_loop +
rel_y_loop * rel_y_loop)
self.loop_angle = float(temp_loop_angle[0])
# Check if the robot has moved behing him
if (abs(wraptopi(self.angle_goal + np.pi - self.loop_angle)) <
1.0) and temp_loop_distance > 1:
self.overwrite_and_reverse_direction = True
# if another bot is close and priority is lower than the other bot, stop moving
if bot_is_close and priority is False:
#GRADIENTBUG: See if bearing goal needs flipping or something else
if outbound:
if (goal_angle_other < 0 and self.angle_goal < 0) or (
goal_angle_other > 0 and self.angle_goal > 0):
self.angle_goal = -1 * self.angle_goal
self.goal_angle_dir = wraptopi(current_heading -
self.angle_goal)
#self.angle_goal = self.angle_goal
'''if bot_is_really_close:
self.state = self.transition("MOVE_OUT_OF_WAY")'''
pass
bearing_to_goal = wraptopi(self.angle_goal - current_heading)
goal_accesability_check = False
if (self.direction == -1):
goal_accesability_check = bearing_to_goal < 0 and bearing_to_goal > -1.57
else:
goal_accesability_check = bearing_to_goal > 0 and bearing_to_goal < 1.57
# If it is rotating around a wall, front range is free and it is close to the angle_goal
if self.state_WF is "ROTATE_AROUND_WALL" or self.state_WF is "ROTATE_AROUND_CORNER":
if front_range > 1.5 and goal_accesability_check:
self.saved_pose = deepcopy(odometry)
self.goal_angle_dir = wraptopi(current_heading -
self.angle_goal)
# Goto rotate_to_goal
self.state = self.transition("ROTATE_TO_GOAL")
pass
if self.state_WF is "WALL_FOLLOWING":
#reset sample gattering
if self.rssi_sample_reset:
self.saved_pose_sample = deepcopy(odometry)
self.rssi_sample_reset = False
self.prev_rssi = rssi_to_tower
rel_x_sample = odometry.pose.position.x - self.saved_pose_sample.pose.position.x
rel_y_sample = odometry.pose.position.y - self.saved_pose_sample.pose.position.y
distance = math.sqrt(rel_x_sample * rel_x_sample +
rel_y_sample * rel_y_sample)
if distance > 1.0:
self.rssi_sample_reset = True
heading_rssi = current_heading
diff_rssi = self.prev_rssi - rssi_to_tower
# print("diff rssi", diff_rssi)
if outbound is False:
self.angle_goal = self.fillHeadingArray(
heading_rssi, diff_rssi, 5)
#GRADIENT implement the gradient search during wallfollowing here!!
# Rotate to Goal
elif self.state == "ROTATE_TO_GOAL":
# If the heading is close to the angle goal
goal_check = self.logicIsCloseTo(current_heading - self.angle_goal,
0.0, 0.1)
if goal_check:
self.state = self.transition("FORWARD")
pass
# Reverse (local) direction
elif self.state == "MOVE_OUT_OF_WAY":
if bot_is_really_close is False or priority is True:
#GRotate to goal
self.state = self.transition("ROTATE_TO_GOAL")
pass
#For debugging in matlab, uncomment this!
# np.savetxt('plot_rssi_array.txt',self.rssi_array,delimiter=',')
# np.savetxt('plot_rssi_heading_array.txt',self.rssi_heading_array,delimiter=',')
# np.savetxt('plot_angle_rssi.txt',[self.angle_rssi, self.rssi_goal_angle_adjust])
#print(self.state)
#################### STATE ACTIONS #####################
#Forward
if self.state == "FORWARD":
#Go forward
twist = self.twistForward()
if from_gazebo:
# If the left or right range is activated during forward, move it away from the wall
#TODO: find out if this is still necessary
if (left_range < self.ref_distance_from_wall):
twist.linear.y = twist.linear.y - 0.2
if (right_range < self.ref_distance_from_wall):
twist.linear.y = twist.linear.y + 0.2
# Wall_follwoing
elif self.state == "WALL_FOLLOWING":
# Use the wallfollowing controller, unique per simulated robot
if from_gazebo is True:
if self.direction is 1:
twist, self.state_WF = self.wall_follower.wall_follower(
front_range, right_range, current_heading,
self.direction)
else:
twist, self.state_WF = self.wall_follower.wall_follower(
front_range, left_range, current_heading,
self.direction)
else:
if self.direction is 1:
twist, self.state_WF = WF_argos.wallFollowingController(
RRT.getRangeRight(), RRT.getRangeFrontRight(),
RRT.getLowestValue(), RRT.getHeading(),
RRT.getArgosTime(), self.direction)
else:
twist, self.state_WF = WF_argos.wallFollowingController(
RRT.getRangeLeft(), RRT.getRangeFrontLeft(),
RRT.getLowestValue(), RRT.getHeading(),
RRT.getArgosTime(), self.direction)
# Reverse the heading command
#TODO check this out why this is the case
twist.angular.z = twist.angular.z * -1
#Rotate to goal
elif self.state == "ROTATE_TO_GOAL":
# To make sure that the robot is turning in the right direction to sae time
if self.goal_angle_dir < 0:
twist = self.twistTurn(self.max_rate)
else:
twist = self.twistTurn(-1 * self.max_rate)
if self.state == "MOVE_OUT_OF_WAY":
if from_gazebo is True:
#TODO implement true move out of way for gazebo, just like the real quadcopter
twist = self.hover()
else:
# In argos, just let the other bot go past you, no need to move out of way
twist = self.hover()
#return twist and the adjusted angle goal by the rssi
return twist, self.rssi_goal_angle_adjust, self.angle_goal
class ComAngleLoopController:
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
max_speed = 0.2
front_range = 0.0
right_range = 0.0
max_rate = 0.5
state_start_time = 0
state = "FORWARD"
previous_heading = 0.0
angle = 2000
calculate_angle_first_time = True
around_corner_first_turn = True
heading_prev = 0.0
heading = 0.0
first_scan = True
scan_obstacle_done = False
scan_obstacle_array = []
scan_angle_array = []
wall_angle = 0
first_run = True
prev_distance = 1000.0
already_reversed_direction = False
state_WF = ""
direction = 1
def init(self, new_ref_distance_from_wall, max_speed_ref=0.2):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.state = "FORWARD"
self.max_speed = max_speed_ref
self.first_run = True
def take_off(self):
twist = Twist()
twist.linear.z = 0.1
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurn(self, rate):
v = 0
w = rate
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurnCircle(self, radius):
v = self.max_speed
w = self.direction * (-v / radius)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def calculateWallRANSAC(self, range_ptx, angle_pty, scan_angle):
points_ptx = np.zeros(len(angle_pty))
points_pty = np.zeros(len(angle_pty))
for it in range(0, len(angle_pty)):
points_ptx[it] = math.sin(angle_pty[it]) * range_ptx[it]
points_pty[it] = math.cos(angle_pty[it]) * range_ptx[it]
if np.isnan(points_ptx[it]) or np.isinf(points_ptx[it]):
points_ptx[it] = 0
if np.isnan(points_pty[it]) or np.isinf(points_pty[it]):
points_pty[it] = 0
ransac = linear_model.RANSACRegressor()
ransac.fit(points_ptx.reshape(-1, 1), points_pty.reshape(-1, 1))
inlier_mask = ransac.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
line_y_ransac = ransac.predict(points_ptx.reshape(-1, 1))
wall_angle = ransac.estimator_.coef_[0][0]
print(wall_angle)
#plt.plot(points_ptx,points_pty)
# plt.hold(True)
#plt.plot(points_ptx,line_y_ransac);
#plt.ylim(0,1.1)
#plt.show()
#time.sleep(10)
return wall_angle
def logicIsCloseTo(self, real_value=0.0, checked_value=0.0, margin=0.05):
if real_value > checked_value - margin and real_value < checked_value + margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = time.time()
return state
def stateMachine(self, front_range, right_range, left_range,
current_heading, angle_goal, distance_goal):
twist = Twist()
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
if left_range == None:
left_range = inf
self.heading = current_heading
if self.first_run is True:
self.prev_distance = distance_goal
self.first_run = False
print("direction ", self.direction)
print("ranges ", left_range, right_range, front_range)
# Handle State transition
if self.state == "FORWARD":
if front_range < self.ref_distance_from_wall + 0.2:
self.state = self.transition("HOVER")
self.wall_follower.init(self.ref_distance_from_wall,
self.max_speed)
self.heading_prev = self.heading
self.first_scan = True
self.scan_obstacle_done = False
self.scan_obstacle_array = []
self.scan_angle_array = []
self.direction = 1
self.already_reversed_direction = False
if self.state == "HOVER":
if time.time() - self.state_start_time > 1:
self.state = self.transition("SCAN_OBSTACLE")
if self.state == "SCAN_OBSTACLE":
if self.scan_obstacle_done is True:
self.wall_angle = self.calculateWallRANSAC(
self.scan_obstacle_array, self.scan_angle_array, 0.52)
self.state = "WALL_FOLLOWING"
if self.state == "REVERSE_DIRECTION":
if front_range < self.ref_distance_from_wall + 0.2:
self.state = self.transition("WALL_FOLLOWING")
if self.direction == -1:
self.wall_angle = -1
elif self.direction == 1:
self.wall_angle = 1
elif self.state == "WALL_FOLLOWING":
#print(self.heading,self.heading_prev,wraptopi(self.heading-self.heading_prev),angle_goal)
if self.logicIsCloseTo(
current_heading, wraptopi(angle_goal), 0.05
) and front_range > 1.4 and self.state_WF is "ROTATE_AROUND_WALL":
#if self.heading < self.heading_prev and front_range > 1.2:
self.state = "FORWARD"
if self.prev_distance < distance_goal and self.already_reversed_direction is False:
self.state = self.transition("REVERSE_DIRECTION")
self.already_reversed_direction = True
# Handle actions
if self.state == "FORWARD":
twist = self.twistForward()
if self.state == "HOVER":
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0
if self.state == "SCAN_OBSTACLE":
scan_angle = 0.52
if self.first_scan is True:
twist = self.twistTurn(-0.5)
if self.logicIsCloseTo(
wraptopi(self.heading_prev - scan_angle),
current_heading, 0.1):
self.first_scan = False
else:
self.scan_obstacle_array.append(front_range)
self.scan_angle_array.append(
wraptopi(self.heading_prev - current_heading))
twist = self.twistTurn(0.5)
if self.logicIsCloseTo(
wraptopi(self.heading_prev + scan_angle),
current_heading, 0.1):
self.scan_obstacle_done = True
if self.state == "REVERSE_DIRECTION":
twist = self.twistTurn(self.direction * -0.5)
elif self.state == "WALL_FOLLOWING":
if self.wall_angle <= 0:
self.direction = 1
twist, self.state_WF = self.wall_follower.wall_follower(
front_range, right_range, current_heading, self.direction)
else:
self.direction = -1
twist, self.state_WF = self.wall_follower.wall_follower(
front_range, left_range, current_heading, self.direction)
print(self.state)
self.lastTwist = twist
return twist
class GradientBugController:
#First run of the algorithm
first_run = True
#For wall follower
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
already_reversed_direction=False
direction = 1
#For state transitions
state_start_time = 0
state = "FORWARD"
state_WF = ""
#Max values
max_speed = 0.2
max_rate = 0.5
# previous values
heading_prev = 0.0
prev_distance = 1000.0
#For RSSI measurements
do_circle = True
angle_rssi = 0
rssi_array=[]
rssi_heading_array =[]
rssi_goal_angle_adjust = 0;
rssi_linear_array = []
rssi_linear_array_max_size = 49
prev_rssi = 0
t = 0
saved_pose = PoseStamped()
saved_pose_hit = PoseStamped()
overwrite_and_reverse_direction = False
not_out_of_the_woods = True
def init(self,new_ref_distance_from_wall,max_speed_ref = 0.2, max_rate_ref = 0.5):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.state = "ROTATE_360"
self.max_speed = max_speed_ref
self.max_rate = max_rate_ref
self.rssi_goal_angle_adjust = 0
self.first_run = True
self.do_circle = True
self.rssi_array = []
self.rssi_heading_array = []
self.rssi_linear_array = []
self.state_start_time = 0
self.angle_rssi=0
self.heading_prev = 0
self.prev_distance = 1000
self.already_reversed_direction=False
self.direction = 1
self.saved_pose = PoseStamped()
self.saved_pose_hit = PoseStamped()
self.overwrite_and_reverse_direction = False
self.not_out_of_the_woods = True
self.loop_angle=0
def take_off(self):
twist = Twist()
twist.linear.z = 0.1;
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurn(self, rate):
v = 0
w = rate
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurnCircle(self, radius):
v = self.max_speed
w = self.direction*(-v/radius)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistForwardAlongWall(self, range):
twist = Twist()
twist.linear.x = self.max_speed
if self.logicIsCloseTo(self.ref_distance_from_wall, range, 0.1) == False:
if range>self.ref_distance_from_wall:
twist.linear.y = self.direction*( - self.max_speed/3)
else:
twist.linear.y = self.direction*self.max_speed /3
return twist
def logicIsCloseTo(self,real_value = 0.0, checked_value =0.0, margin=0.05):
if real_value> checked_value-margin and real_value< checked_value+margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = self.simulator_time
return state
def stateMachine(self, front_range, right_range, left_range, current_heading, angle_goal, distance_goal, rssi_to_tower,odometry, correct_time, from_gazebo = True, WF_argos = None, RRT= None, outbound = False):
#Initialization of the twist command
twist = Twist()
#Save simulator time as the correct time
self.simulator_time = correct_time;
# Deal with none values of the range sensors
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
if left_range == None:
left_range = inf
# First thing to take care of at the very first run
if self.first_run is True:
self.prev_distance = 2000;#distance_goal;
self.angle_rssi = 2000;
self.first_run = False
self.heading_prev=0#current_heading
self.state_start_time = correct_time
#if angle_goal is not 2000:
# self.angle_rssi = current_heading - angle_goal
# Bearing to goal is the angle to goal
#TODO check if this is also correct for the gazebo implementation....
bearing = angle_goal;
bearing_with_adjust = angle_goal#current_heading - angle_goal+self.angle_rssi;#self.rssi_goal_angle_adjust;
self.heading = current_heading;
#################### STATE TRANSITIONS#####################
# Forward
if self.state == "FORWARD":
if self.do_circle == False:
self.rssi_linear_array.append(rssi_to_tower-self.prev_rssi)
if len(self.rssi_linear_array)>self.rssi_linear_array_max_size:
del self.rssi_linear_array[0]
#print("mean rssi", np.mean(self.rssi_linear_array))
#print("rssi", rssi_to_tower , self.prev_rssi)
if np.mean(self.rssi_linear_array)>0:
# print(self.rssi_linear_array)
self.do_circle = True
if ((self.logicIsCloseTo(self.saved_pose.pose.position.x, odometry.pose.position.x,1)==True ) and \
(self.logicIsCloseTo(self.saved_pose.pose.position.y, odometry.pose.position.y,1)==True)):
self.not_out_of_the_woods = True
else:
self.not_out_of_the_woods = False
#If need to do circle and 1 second has passed
if self.do_circle == True and correct_time-self.state_start_time > 1:
# Initialize rssi and heading arrays and save previous heading
self.rssi_array = []
self.rssi_heading_array = []
self.heading_prev=current_heading
# Go to rotate_360
self.state = self.transition("ROTATE_360")
pass
#If front range is activated to be close et the other wall
if front_range < self.ref_distance_from_wall+0.2:
#Initialize the wallfollower
if from_gazebo:
self.wall_follower.init(self.ref_distance_from_wall,self.max_speed)
else:
WF_argos.init()
# save previous heading and initialize the reverse option
self.heading_prev = current_heading
self.already_reversed_direction = False
# To evaluate the wall angle for the local direction (replace scan_obstacle)
if self.overwrite_and_reverse_direction and self.not_out_of_the_woods == False:
print("overwrite direction!")
self.direction = -1*self.direction
self.overwrite_and_reverse_direction = False
else:
rand_num = random.randint(1,101)
print(rand_num)
print(left_range,right_range)
if left_range<right_range and left_range < 2.0:
if rand_num<70:
self.direction = -1
else:
self.direction = 1
elif left_range>right_range and right_range < 2.0:
if rand_num<70:
self.direction = 1
else:
self.direction = -1
elif left_range>2.0 and right_range>2.0:
if rand_num<50:
self.direction = 1
else:
self.direction = -1
else:
if rand_num<50:
self.direction = -1
else:
self.direction = 1
self.saved_pose_hit = deepcopy(odometry)
#Go to wall_following
self.state = self.transition("WALL_FOLLOWING")
pass
# Reverse (local) direction
elif self.state =="REVERSE_DIRECTION":
# if the front range sensor is activated, go back to wall_following in the other direction
if front_range < self.ref_distance_from_wall+0.2:
# Reverse local direction flag
if self.direction == -1:
self.direction = 1
elif self.direction == 1:
self.direction = -1
if from_gazebo:
self.wall_follower.init(self.ref_distance_from_wall,self.max_speed)
else:
WF_argos.init()
#Go to wall_following
self.state = self.transition("WALL_FOLLOWING")
pass
# Wall Following
elif self.state == "WALL_FOLLOWING":
rel_x_loop = odometry.pose.position.x- self.saved_pose_hit.pose.position.x
rel_y_loop = odometry.pose.position.y - self.saved_pose_hit.pose.position.y
temp_loop_angle = wraptopi(np.arctan2(rel_y_loop,rel_x_loop))
self.loop_angle =float(temp_loop_angle[0])
# If it is rotating around a wall, front range is free and it is close to the angle_goal
if self.state_WF is "ROTATE_AROUND_WALL" or self.state_WF is "ROTATE_AROUND_CORNER":
#if front_range>1.5 and (bearing_with_adjust>-0.2 and bearing_with_adjust < 0.2):
#if front_range>1.5 and ((current_heading-self.angle_rssi)>-0.2 and (current_heading-self.angle_rssi) < 0.2): #version 1
if front_range>1.5 and (abs(wraptopi(self.angle_rssi-current_heading))<0.2): #version 2
# Indicate that the rssi finding circle needs to be made
self.do_circle = True
self.prev_distance = deepcopy(distance_goal)
# CHeck if has been on a position before
#if ((self.logicIsCloseTo(self.saved_pose.pose.position.x, odometry.pose.position.x,1)==True ) and \
# (self.logicIsCloseTo(self.saved_pose.pose.position.y, odometry.pose.position.y,1)==True)):
# print("yes!")
# Check if the robot has moved behing him
if abs(wraptopi(self.angle_rssi+np.pi-self.loop_angle))<0.5:
self.overwrite_and_reverse_direction = True
self.saved_pose = deepcopy(odometry)
#Save previous distance for reverse direction possibility
# Goto rotate_to_goal
self.state = self.transition("ROTATE_TO_GOAL")
pass
# If the previous saved distance is smaller than the current one and it hasn't reverse direction yet
#if self.prev_distance+2.0<distance_goal and self.already_reversed_direction is False: #version 1
'''if ((self.logicIsCloseTo(self.saved_pose.pose.position.x, odometry.pose.position.x,0.05)!=True ) or \
(self.logicIsCloseTo(self.saved_pose.pose.position.y, odometry.pose.position.y,0.05)!=True)) \
and self.already_reversed_direction is False: #version 2
# Already reversed direction to prevent it from happinening again during the wallfolowing
self.already_reversed_direction = True
# Go to reverse_direction
self.state = self.transition("REVERSE_DIRECTION")'''
# Rotate to Goal
elif self.state=="ROTATE_TO_GOAL":
# If the heading is close to the angle goal
#if (self.angle_rssi is 2000 and self.logicIsCloseTo(bearing_with_adjust,0,0.1)) or (self.angle_rssi is not 2000 and self.logicIsCloseTo(bearing_with_adjust,0,0.1)):
if (self.angle_rssi is 2000 and self.logicIsCloseTo(angle_goal,0,0.1)) or (self.angle_rssi is not 2000 and self.logicIsCloseTo(current_heading,self.angle_rssi,0.1)):
#
#Go to forward
self.rssi_linear_array=[]
self.state = self.transition("FORWARD")
pass
#Rotate 360
elif self.state=="ROTATE_360":
# if 2 seconds has passed, the previous heading is close to the current heading and do_circle flag is on
# print("check rotate",correct_time,self.state_start_time,current_heading,self.heading_prev,self.do_circle)
if correct_time-self.state_start_time > 3 and self.logicIsCloseTo(current_heading,wraptopi( self.heading_prev),0.1) and self.do_circle:
#do_circle flag is on false so it knows it is finished
self.do_circle = False
#To check if it's not in a "dead zone"
if(np.mean(self.rssi_array)<-44):
#Filter the saved rssi array
rssi_array_filt = medfilt(self.rssi_array,9)
#Find the maximum RSSI and it's index
index_max_rssi =np.argmax(rssi_array_filt)
# Retrieve the offset angle to the goal
self.angle_rssi =wraptopi(self.rssi_heading_array[index_max_rssi]+3.14)
# Determine the adjusted goal angle, which is added to the heading later
self.rssi_goal_angle_adjust = wraptopi(angle_goal-self.angle_rssi)
# Go to rotate to goal
self.state = self.transition("ROTATE_TO_GOAL")
pass
#For debugging in matlab, uncomment this!
# np.savetxt('plot_rssi_array.txt',self.rssi_array,delimiter=',')
# np.savetxt('plot_rssi_heading_array.txt',self.rssi_heading_array,delimiter=',')
# np.savetxt('plot_angle_rssi.txt',[self.angle_rssi, self.rssi_goal_angle_adjust])
#print(self.direction)
# print(self.state)
#################### STATE ACTIONS #####################
#Forward
if self.state == "FORWARD":
#Go forward
twist=self.twistForward()
if from_gazebo:
# If the left or right range is activated during forward, move it away from the wall
#TODO: find out if this is still necessary
if(left_range<self.ref_distance_from_wall):
twist.linear.y = twist.linear.y-0.2;
if(right_range<self.ref_distance_from_wall):
twist.linear.y = twist.linear.y+0.2;
# Reverse direction
if self.state =="REVERSE_DIRECTION":
# Just turn towards the wall
twist = self.twistTurn(self.direction*-0.5)
# Wall_follwoing
elif self.state == "WALL_FOLLOWING":
# Use the wallfollowing controller, unique per simulated robot
if from_gazebo is True:
if self.direction is 1:
twist, self.state_WF = self.wall_follower.wall_follower(front_range,right_range, current_heading,self.direction)
else:
twist, self.state_WF = self.wall_follower.wall_follower(front_range,left_range, current_heading,self.direction)
else:
if self.direction is 1:
twist, self.state_WF = WF_argos.wallFollowingController(RRT.getRangeRight(),RRT.getRangeFrontRight(), RRT.getLowestValue(),RRT.getHeading(),RRT.getArgosTime(),self.direction)
else:
twist, self.state_WF = WF_argos.wallFollowingController(RRT.getRangeLeft(),RRT.getRangeFrontLeft(), RRT.getLowestValue(),RRT.getHeading(),RRT.getArgosTime(),self.direction)
# Reverse the heading command
#TODO check this out why this is the case
twist.angular.z = twist.angular.z*-1
#Rotate to goal
elif self.state=="ROTATE_TO_GOAL":
# To make sure that the robot is turning in the right direction to sae time
if (self.angle_rssi-current_heading)>0:
twist = self.twistTurn(self.max_rate)
else:
twist = self.twistTurn(-1*self.max_rate)
# Rotate 360 degrees
elif self.state =="ROTATE_360":
twist = self.twistTurn(0)
# If do_circle flag is on, save the rssi and angle goal in a array
if self.do_circle is True and correct_time-self.state_start_time > 1:
self.rssi_array.append(rssi_to_tower)
self.rssi_heading_array.append(current_heading)
#Turn with max_rate
twist = self.twistTurn(self.max_rate*0.5)
#Save previous RSSI for next loop
self.prev_rssi = rssi_to_tower
#return twist and the adjusted angle goal by the rssi
return twist, self.rssi_goal_angle_adjust
class WallFollowerController:
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
max_speed = 0.2
front_range = 0.0
right_range = 0.0
max_rate = 0.5
state_start_time = 0
state = "FORWARD"
previous_heading = 0.0
angle = 2000
calculate_angle_first_time = True
around_corner_first_turn = True
def init(self, new_ref_distance_from_wall, max_speed_ref=0.2):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.max_speed = max_speed_ref
self.state = "FORWARD"
def take_off(self):
twist = Twist()
twist.linear.z = 0.1
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def logicIsCloseTo(self, real_value=0.0, checked_value=0.0, margin=0.05):
if real_value > checked_value - margin and real_value < checked_value + margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = time.time()
return state
def stateMachine(self, front_range, right_range, left_range,
current_heading, angle_goal, distance_goal):
twist = Twist()
state_WF = "NONE"
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
# Handle State transition
if self.state == "FORWARD":
if front_range < self.ref_distance_from_wall + 0.2:
self.state = self.transition("WALL_FOLLOWING")
self.wall_follower.init(self.ref_distance_from_wall,
self.max_speed)
# Handle actions
if self.state == "FORWARD":
twist = self.twistForward()
elif self.state == "WALL_FOLLOWING":
twist, state_WF = self.wall_follower.wall_follower(
front_range, right_range, current_heading, 1)
#twist.linear.x = 0.0;
#twist.linear.y = 0.0;
#twist.angular.z = 0.0;
print(self.state)
self.lastTwist = twist
return twist, state_WF
def crazyFlieloop(self):
wall_follower = WallFollower()
wall_follower.init(0.5)
twist = Twist()
#log_config.data_received_cb.add_callback(self.data_received)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
lg_states = LogConfig(name='kalman_states', period_in_ms=100)
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
state= "forward"
with SyncCrazyflie(self.URI, cf=cf) as scf:
with MotionCommander(scf,0.8) as motion_commander:
with MultiRanger(scf) as multi_ranger:
with Stabilization(scf) as stabilization:
with SyncLogger(scf, lg_states) as logger_states:
keep_flying = True
twist.linear.x = 0.2
twist.linear.y = 0.0
twist.angular.z = 0
heading_prev = 0.0
heading = 0.0
angle_to_goal = 0.0;
kalman_x = 0.0
kalman_y = 0.0
already_reached_far_enough = False
while keep_flying:
for log_entry_1 in logger_states:
data = log_entry_1[1]
heading = math.radians(float(data["stabilizer.yaw"]));
kalman_x =float(data["kalman_states.ox"])
kalman_y =float(data["kalman_states.oy"])
if already_reached_far_enough:
angle_to_goal = wraptopi(np.pi+math.atan(kalman_y/kalman_x))
else:
angle_to_goal = wraptopi(math.atan(kalman_y/kalman_x))
break
time.sleep(0.1)
if state == "forward":
print(kalman_x, kalman_y, math.sqrt(math.pow(kalman_x,2) + math.pow(kalman_y,2)))
print(state)
if multi_ranger.front < 0.5 and multi_ranger.front is not None:
state="wall_following"
wall_follower.init(0.5)
if math.sqrt(math.pow(kalman_x,2) + math.pow(kalman_y,2)) > 8 and already_reached_far_enough is False:
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.angular.z = 0#np.pi*2 / 6
state = "stop"#"turn_to_target"
heading_prev = heading
already_reached_far_enough = True
if math.sqrt(math.pow(kalman_x,2) + math.pow(kalman_y,2)) < 0.4:
state = "stop"
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.angular.z = 0.0
elif state == "wall_following":
twist = wall_follower.wall_follower(multi_ranger.front,multi_ranger.right,stabilization.heading)
if wraptopi(heading-heading_prev) < angle_to_goal and multi_ranger.front > 1.2 :
state = "forward"
elif state == "turn_to_target":
if wraptopi(heading-heading_prev) < angle_to_goal:
twist.linear.x = 0.2
twist.linear.y = 0.0
twist.angular.z = 0.0
state = "forward"
elif state == "stop":
break
motion_commander._set_vel_setpoint(twist.linear.x,twist.linear.y,0,-1*math.degrees(twist.angular.z))
if multi_ranger.up < 0.2 and multi_ranger.up is not None:
print("up range is activated")
keep_flying = False
motion_commander.stop()
print("demo terminated")
class GradientBugController:
#First run of the algorithm
first_run = True
#For wall follower
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
already_reversed_direction=False
direction = 1
#For state transitions
state_start_time = 0
state = "FORWARD"
state_WF = ""
#Max values
max_speed = 0.2
max_rate = 0.5
# previous values
heading_prev = 0.0
prev_distance = 1000.0
#For RSSI measurements
do_circle = True
angle_rssi = 0
rssi_array=[]
rssi_heading_array =[]
rssi_goal_angle_adjust = 0;
rssi_linear_array = []
rssi_linear_array_max_size = 49
prev_rssi = 0
t = 0
saved_pose = PoseStamped()
saved_pose_hit = PoseStamped()
overwrite_and_reverse_direction = False
not_out_of_the_woods = True
def init(self,new_ref_distance_from_wall,max_speed_ref = 0.2, max_rate_ref = 0.5):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.state = "ROTATE_TO_GOAL"
self.max_speed = max_speed_ref
self.max_rate = max_rate_ref
self.rssi_goal_angle_adjust = 0
self.first_run = True
self.do_circle = True
self.rssi_array = []
self.rssi_heading_array = []
self.rssi_linear_array = []
self.state_start_time = 0
self.angle_rssi=0
self.heading_prev = 0
self.prev_distance = 1000
self.already_reversed_direction=False
self.direction = 1
self.saved_pose = PoseStamped()
self.saved_pose_hit = PoseStamped()
self.overwrite_and_reverse_direction = False
self.not_out_of_the_woods = True
self.loop_angle=0
def take_off(self):
twist = Twist()
twist.linear.z = 0.1;
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurn(self, rate):
v = 0
w = rate
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistTurnCircle(self, radius):
v = self.max_speed
w = self.direction*(-v/radius)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def twistForwardAlongWall(self, range):
twist = Twist()
twist.linear.x = self.max_speed
if self.logicIsCloseTo(self.ref_distance_from_wall, range, 0.1) == False:
if range>self.ref_distance_from_wall:
twist.linear.y = self.direction*( - self.max_speed/3)
else:
twist.linear.y = self.direction*self.max_speed /3
return twist
def logicIsCloseTo(self,real_value = 0.0, checked_value =0.0, margin=0.05):
if real_value> checked_value-margin and real_value< checked_value+margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = self.simulator_time
return state
def stateMachine(self, front_range, right_range, left_range, current_heading, bearing_goal, distance_goal, rssi_to_tower,odometry, correct_time, from_gazebo = True, WF_argos = None, RRT= None, outbound = False, bot_is_close = False):
#Initialization of the twist command
twist = Twist()
#Save simulator time as the correct time
self.simulator_time = correct_time;
# Deal with none values of the range sensors
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
if left_range == None:
left_range = inf
# First thing to take care of at the very first run
if self.first_run is True:
self.prev_distance = 2000;#distance_goal;
self.angle_rssi = 2000;
self.first_run = False
self.heading_prev=0#current_heading
self.state_start_time = correct_time
# Bearing to goal is the angle to goal
#TODO check if this is also correct for the gazebo implementation....
#################### STATE TRANSITIONS#####################
# Forward
if self.state == "FORWARD":
#If front range is activated to be close et the other wall
if front_range < self.ref_distance_from_wall+0.2:
#Initialize the wallfollower
if from_gazebo:
self.wall_follower.init(self.ref_distance_from_wall,self.max_speed)
else:
WF_argos.init()
# save previous heading and initialize the reverse option
self.heading_prev = deepcopy(current_heading)
# First check if the bug has not gotten himself in a loop
if self.overwrite_and_reverse_direction:
self.direction = -1*self.direction
self.overwrite_and_reverse_direction = False
else:
# To evaluate the wall angle for the local direction (replace scan_obstacle)
rand_num = random.randint(1,101)
print(rand_num)
print(left_range,right_range)
if left_range<right_range and left_range < 2.0:
if rand_num<70:
self.direction = -1
else:
self.direction = 1
elif left_range>right_range and right_range < 2.0:
if rand_num<70:
self.direction = 1
else:
self.direction = -1
elif left_range>2.0 and right_range>2.0:
if rand_num<50:
self.direction = 1
else:
self.direction = -1
else:
if rand_num<50:
self.direction = -1
else:
self.direction = 1
#Save the hitpoint for loop checking
self.saved_pose_hit = deepcopy(odometry)
#Go to wall_following
self.state = self.transition("WALL_FOLLOWING")
self.already_reversed_direction = False
pass
# Wall Following
elif self.state == "WALL_FOLLOWING":
#Detect the relative position of the robot and the hitpoint to dietect loops
rel_x_loop = odometry.pose.position.x- self.saved_pose_hit.pose.position.x
rel_y_loop = odometry.pose.position.y - self.saved_pose_hit.pose.position.y
temp_loop_angle = wraptopi(np.arctan2(rel_y_loop,rel_x_loop))
self.loop_angle =float(temp_loop_angle[0])
if bot_is_close and self.already_reversed_direction is False:
self.already_reversed_direction=True
self.state = self.transition("REVERSE_DIRECTION")
pass
# If it is rotating around a wall, front range is free and it is close to the angle_goal
if self.state_WF is "ROTATE_AROUND_WALL" or self.state_WF is "ROTATE_AROUND_CORNER":
if front_range>1.5 and (abs(wraptopi(bearing_goal))<0.2): #version 2
# Check if the robot has moved behing him
if (abs(wraptopi(current_heading+bearing_goal+np.pi-self.loop_angle))<0.5):
self.overwrite_and_reverse_direction = True
#print("LOOPING!")
self.saved_pose = deepcopy(odometry)
#Save previous distance for reverse direction possibility
# Goto rotate_to_goal
self.state = self.transition("ROTATE_TO_GOAL")
pass
# If the previous saved distance is smaller than the current one and it hasn't reverse direction yet
# Rotate to Goal
elif self.state=="ROTATE_TO_GOAL":
# If the heading is close to the angle goal
if( self.logicIsCloseTo(bearing_goal,0,0.2)):
self.state = self.transition("FORWARD")
# Reverse (local) direction
elif self.state =="REVERSE_DIRECTION":
# if the front range sensor is activated, go back to wall_following in the other direction
if front_range < self.ref_distance_from_wall+0.2:
# Reverse local direction flag
if self.direction == -1:
self.direction = 1
elif self.direction == 1:
self.direction = -1
if from_gazebo:
self.wall_follower.init(self.ref_distance_from_wall,self.max_speed)
else:
WF_argos.init()
#Go to wall_following
self.state = self.transition("WALL_FOLLOWING")
pass
#For debugging in matlab, uncomment this!
# np.savetxt('plot_rssi_array.txt',self.rssi_array,delimiter=',')
# np.savetxt('plot_rssi_heading_array.txt',self.rssi_heading_array,delimiter=',')
# np.savetxt('plot_angle_rssi.txt',[self.angle_rssi, self.rssi_goal_angle_adjust])
#print(self.state)
#################### STATE ACTIONS #####################
#Forward
if self.state == "FORWARD":
#Go forward
twist=self.twistForward()
if from_gazebo:
# If the left or right range is activated during forward, move it away from the wall
#TODO: find out if this is still necessary
if(left_range<self.ref_distance_from_wall):
twist.linear.y = twist.linear.y-0.2;
if(right_range<self.ref_distance_from_wall):
twist.linear.y = twist.linear.y+0.2;
# Wall_follwoing
elif self.state == "WALL_FOLLOWING":
# Use the wallfollowing controller, unique per simulated robot
if from_gazebo is True:
if self.direction is 1:
twist, self.state_WF = self.wall_follower.wall_follower(front_range,right_range, current_heading,self.direction)
else:
twist, self.state_WF = self.wall_follower.wall_follower(front_range,left_range, current_heading,self.direction)
else:
if self.direction is 1:
twist, self.state_WF = WF_argos.wallFollowingController(RRT.getRangeRight(),RRT.getRangeFrontRight(), RRT.getLowestValue(),RRT.getHeading(),RRT.getArgosTime(),self.direction)
else:
twist, self.state_WF = WF_argos.wallFollowingController(RRT.getRangeLeft(),RRT.getRangeFrontLeft(), RRT.getLowestValue(),RRT.getHeading(),RRT.getArgosTime(),self.direction)
# Reverse the heading command
#TODO check this out why this is the case
twist.angular.z = twist.angular.z*-1
#Rotate to goal
elif self.state=="ROTATE_TO_GOAL":
# To make sure that the robot is turning in the right direction to sae time
if (self.angle_rssi-current_heading)>0:
twist = self.twistTurn(self.max_rate)
else:
twist = self.twistTurn(-1*self.max_rate)
if self.state =="REVERSE_DIRECTION":
# Just turn towards the wall
twist = self.twistTurn(self.direction*-0.5)
#return twist and the adjusted angle goal by the rssi
return twist, self.rssi_goal_angle_adjust
class ComController:
wall_follower = WallFollower()
ref_distance_from_wall = 1.0
max_speed = 0.2
front_range = 0.0
right_range = 0.0
max_rate = 0.5
state_start_time = 0
state = "FORWARD"
previous_heading = 0.0;
angle=2000
calculate_angle_first_time = True;
around_corner_first_turn = True;
heading_prev = 0.0
heading = 0.0
def init(self,new_ref_distance_from_wall,max_speed_ref = 0.2):
self.ref_distance_from_wall = new_ref_distance_from_wall
self.state = "FORWARD"
self.max_speed = max_speed_ref
def take_off(self):
twist = Twist()
twist.linear.z = 0.1;
return twist
def hover(self):
twist = Twist()
return twist
def twistForward(self):
v = self.max_speed
w = 0
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def logicIsCloseTo(self,real_value = 0.0, checked_value =0.0, margin=0.05):
if real_value> checked_value-margin and real_value< checked_value+margin:
return True
else:
return False
# Transition state and restart the timer
def transition(self, newState):
state = newState
self.state_start_time = time.time()
return state
def stateMachine(self, front_range, right_range, current_heading, angle_goal, distance_goal):
twist = Twist()
if front_range == None:
front_range = inf
if right_range == None:
right_range = inf
self.heading = current_heading;
# Handle State transition
if self.state == "FORWARD":
if front_range < 1.0:#self.ref_distance_from_wall:
self.state = self.transition("WALL_FOLLOWING")
self.wall_follower.init(self.ref_distance_from_wall,self.max_speed)
self.heading_prev = self.heading
elif self.state == "WALL_FOLLOWING":
#print(self.heading,self.heading_prev,wraptopi(self.heading-self.heading_prev),angle_goal)
if self.logicIsCloseTo(current_heading,wraptopi(angle_goal),0.05) and front_range > 1.4 :
#if self.heading < self.heading_prev and front_range > 1.2:
self.state = "FORWARD"
# Handle actions
if self.state == "FORWARD":
twist=self.twistForward()
elif self.state == "WALL_FOLLOWING":
twist = self.wall_follower.wall_follower(front_range,right_range, current_heading)
print(self.state)
self.lastTwist = twist
return twist