class BlindBugController:
state = "ROTATE_TO_GOAL"
stateStartTime=0
WF=wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0;
first_rotate = True
hitpoint = PoseStamped()
heading_before_turning = 0
hit_points = []
direction = 1
init_direction = 1
last_diff_heading = 0
time_side= -1;
heading_target = 0;
rotated_half_once = False
def __init__(self):
self.distance_to_wall=self.WF.getWantedDistanceToWall();
self.direction = self.WF.getDirectionTurn();
self.init_direction = self.WF.getDirectionTurn();
self.stateStartTime = 0;
self.first_rotate = True;
self.heading_before_turning = 0;
self.time_side = -1
self.heading_target = 0;
self.last_diff_heading = 0;
self.state = "ROTATE_TO_GOAL"
def stateMachine(self, RRT):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front=self.RRT.getRangeFrontLeft()
range_side=self.RRT.getRangeLeft()
elif self.direction is -1:
range_front=self.RRT.getRangeFrontRight()
range_side=self.RRT.getRangeRight()
diff_heading = self.wrap_pi(self.heading_target-self.RRT.getHeading())
print self.time_side
print diff_heading
print self.direction
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1: #If an obstacle comes within the distance of the wall
if self.time_side>0:
self.direction = -1
else:
self.direction = 1
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
if self.direction is 1:
self.time_side = self.time_side + 1*math.sin(diff_heading)
elif self.direction is -1:
self.time_side = self.time_side + 1*math.sin(diff_heading)
#If wall is lost by corner, rotate to goal again
if range_front>=2.0:
self.transition("ROTATE_TO_GOAL")
self.last_diff_heading = diff_heading
self.WF.init()
elif self.state=="ROTATE_TO_GOAL":
if self.logicIsCloseTo(self.heading_target,self.RRT.getHeading(),0.05) and self.first_rotate== False:
self.transition("FORWARD")
if self.logicIsCloseTo(0,self.RRT.getUWBBearing(),0.05) and self.first_rotate:
self.heading_target = self.RRT.getHeading();
self.first_rotate = False
self.direction = self.init_direction
self.transition("FORWARD")
else:
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1:
self.transition("WALL_FOLLOWING")
self.first_rotate = False
elif self.state=="ROTATE_180":
if math.fabs(self.wrap_pi(self.RRT.getHeading()-self.heading_before_turning))>3.04:
self.rotated_half_once = True
self.transition("WALL_FOLLOWING")
# Handle actions
if self.state == "FORWARD":
twist=self.WF.twistForward() #Go forward with maximum speed
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side,range_front,
self.RRT.getLowestValue(),self.RRT.getHeading(),self.RRT.getArgosTime(),self.direction)
elif self.state=="ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
if self.first_rotate or\
(self.last_diff_heading<0 and self.direction == 1) or\
(self.last_diff_heading>0 and self.direction == -1):
twist = self.WF.twistTurnInCorner(self.direction)
else:
if (self.RRT.getArgosTime() - self.stateStartTime)<self.WF.getDistanceAroundCorner90()/0.35 * 10:
twist=self.WF.twistForward()
else:
twist = self.WF.twistTurnAroundCorner(self.distance_to_wall+0.4,self.direction)
elif self.state=="ROTATE_180":
twist = self.WF.twistTurnInCorner(self.direction)
print self.state
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
def twistRotateToGoal(self):
v = 0
w = self.MAX_ROTATION_SPEED * numpy.sign(self.RRT.getUWBBearing())
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def wrap_pi(self, angles_rad):
return numpy.mod(angles_rad+numpy.pi, 2.0 * numpy.pi) - numpy.pi
class ComBugController:
WF=wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0;
first_rotate = True
direction = 1
last_bearing = 0
hitpoint = PoseStamped()
stateStartTime=0
state = "ROTATE_TO_GOAL"
def __init__(self):
#Get Desired distance from the wall
self.distance_to_wall=self.WF.getWantedDistanceToWall();
self.direction = self.WF.getDirectionTurn();
self.hitpoint = PoseStamped();
self.first_rotate = True;
self.last_bearing = 0;
self.stateStartTime = 0;
self.state = "ROTATE_TO_GOAL"
self.pose_tower = PoseStamped();
self.first_run = 1
self.current_UWB_bearing = 2000
def stateMachine(self,RRT,odometry):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front=self.RRT.getRangeFrontLeft()
range_side=self.RRT.getRangeLeft()
elif self.direction is -1:
range_front=self.RRT.getRangeFrontRight()
range_side=self.RRT.getRangeRight()
bot_pose = PoseStamped();
bot_pose.pose.position.x = odometry.pose.position.x;
bot_pose.pose.position.y = odometry.pose.position.y;
if self.first_run:
self.bot_init_position = self.RRT.getPoseBot();
pose_tower_abs = self.RRT.getPoseTower();
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x;
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y;
# First topics show it as zero, so wait a bit untill that is not the case
if( abs(pose_tower_abs.pose.position.x)> 0.0):
self.first_run = 0
else:
rel_x = self.pose_tower.pose.position.x-bot_pose.pose.position.x ;
rel_y = self.pose_tower.pose.position.y - bot_pose.pose.position.y ;
theta = -1*self.RRT.getHeading();
rel_loc_x = rel_x*numpy.math.cos(theta)-rel_y*numpy.math.sin(theta)
rel_loc_y = rel_x*numpy.math.sin(theta)+rel_y*numpy.math.cos(theta)
self.current_UWB_bearing = numpy.arctan2(rel_loc_y,rel_loc_x)
#self.current_UWB_bearing = self.RRT.getUWBBearing();
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1: #If an obstacle comes within the distance of the wall
# self.hitpoint = self.RRT.getPoseBot();
self.hitpoint.pose.position.x = odometry.pose.position.x;
self.hitpoint.pose.position.y = odometry.pose.position.y;
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
#bot_pose = self.RRT.getPoseBot();
#If wall is lost by corner, rotate to goal again
if range_front>=2.0 and \
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,0.05)!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,0.05)!=True)):
self.transition("ROTATE_TO_GOAL")
self.last_bearing = deepcopy(self.current_UWB_bearing)
self.WF.init()
elif self.state=="ROTATE_TO_GOAL":
if self.logicIsCloseTo(0,self.current_UWB_bearing,0.1) :
self.first_rotate = False
self.transition("FORWARD")
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1:
self.transition("WALL_FOLLOWING")
self.first_rotate = False
# Handle actions
if self.state == "FORWARD":
twist=self.WF.twistForward() #Go forward with maximum speed
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side,range_front,
self.RRT.getLowestValue(),self.RRT.getHeading(),self.RRT.getArgosTime(),self.direction)
elif self.state=="ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
if self.first_rotate or\
(self.last_bearing<0 and self.direction == 1) or\
(self.last_bearing>0 and self.direction == -1):
twist = self.WF.twistTurnInCorner(self.direction)
else:
if (self.RRT.getArgosTime() - self.stateStartTime)<self.WF.getDistanceAroundCorner90()/0.35 * 10:
twist=self.WF.twistForward()
else:
twist = self.WF.twistTurnAroundCorner(self.distance_to_wall+0.2,self.direction)
print self.state
#self.cmdVelPub.publish(twist)
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
class GradientBugController:
WF = wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
GB = gradient_bug_v1.GradientBugController()
GB2 = gradient_bug_v2.GradientBugController()
GB3 = gradient_bug_v3.GradientBugController()
distance_to_wall = 0;
rssi_goal_angle_adjust = 0
goal_angle = 0
bot_is_close = False
def __init__(self):
#Get Desired distance from the wall
self.distance_to_wall=self.WF.getWantedDistanceToWall();
self.pose_tower = PoseStamped();
self.first_run = 1
#Init embedded gradient bug
# self.GB.init(self.distance_to_wall,self.WF.getMaximumForwardSpeed(),self.WF.getMaximumRotationSpeed())
#self.GB2.init(self.distance_to_wall,self.WF.getMaximumForwardSpeed(),self.WF.getMaximumRotationSpeed())
self.GB3.init(self.distance_to_wall,self.WF.getMaximumForwardSpeed(),self.WF.getMaximumRotationSpeed())
self.current_UWB_range = 0
self.current_UWB_bearing = 0
self.rssi_goal_angle_adjust = 0
def stateMachine(self,RRT,odometry,outbound = False, outbound_angle = 0, own_id=1):
# Get recieve ros topics
self.RRT = RRT
# Get bot position from odometry (with drift)
bot_pose = PoseStamped();
bot_pose.pose.position.x = odometry.pose.position.x;
bot_pose.pose.position.y = odometry.pose.position.y;
#If it is the first loop of the run
if self.first_run:
self.GB3.init(self.distance_to_wall,self.WF.getMaximumForwardSpeed(),self.WF.getMaximumRotationSpeed(),outbound_angle)
self.bot_init_position = self.RRT.getPoseBot(); # Get the initial position of the bot
pose_tower_abs = self.RRT.getPoseTower(); # Get the position of the tower
#Get the relative position to the tower (CHECK IF THIS IS STILL POSSIBLE!)
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x;
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y;
# THIS MUST BE GONE IF WE ARE DOING HOMING AKA STARTING FROM TOWER!!!
#if( abs(pose_tower_abs.pose.position.x)> 0.0):
self.first_run = False
else:
# Get the position to the tower with the bot's odometry
rel_x = self.pose_tower.pose.position.x -bot_pose.pose.position.x
rel_y = self.pose_tower.pose.position.y- bot_pose.pose.position.y
# Rotate the relative position tot tower to the bot's heading
# SEE IF THIS IS STILL NECESSARY!
theta = self.wrap_pi(-self.RRT.getHeading())
rel_loc_x = rel_x*numpy.math.cos(theta)-rel_y*numpy.math.sin(theta)
rel_loc_y = rel_x*numpy.math.sin(theta)-rel_y*numpy.math.cos(theta)
# Make adjusted odometry (FOR DEBUGGIN ONLY!)
save_pos_rel_x = 12+ bot_pose.pose.position.x - self.pose_tower.pose.position.x
save_pos_rel_y = 12+ bot_pose.pose.position.y - self.pose_tower.pose.position.y ;
adjusted_theta = self.wrap_pi(self.rssi_goal_angle_adjust)
rel_x_adjust = save_pos_rel_x*numpy.math.cos(adjusted_theta)-save_pos_rel_y*numpy.math.sin(adjusted_theta)
rel_y_adjust = save_pos_rel_x*numpy.math.sin(adjusted_theta)+save_pos_rel_y*numpy.math.cos(adjusted_theta)
#numpy.savetxt('rel_loc_x.txt',[rel_x_adjust],delimiter=',')
#numpy.savetxt('rel_loc_y.txt',[rel_y_adjust],delimiter=',')
# Get angle to goal and angle to goal
self.current_UWB_angle = self.wrap_pi(numpy.arctan2(rel_y,rel_x))
self.current_UWB_range =12- math.sqrt(math.pow(rel_y,2)+math.pow(rel_x,2))
# the bearing value sometimes comes in the form or an array, just is temp fix!!
if isinstance(self.current_UWB_bearing,numpy.ndarray):
self.current_UWB_bearing=float(self.current_UWB_bearing[0])
#Retrieve the rssi_noise
rssi_noise_list = numpy.random.normal(self.RRT.getRSSITower(),1,1);
rssi_noise = round(float(rssi_noise_list[0]))
#rssi_noise= round(self.RRT.getRSSITower())
if rssi_noise>-43:
rssi_noise = -43
#if outbound == True:
# self.current_UWB_bearing =self.wrap_pi(outbound_angle-self.RRT.getHeading() )
#FOR NOW JUST SUBSTITUTE RANGE FOR TRUE RANGE!!
self.current_range = self.RRT.getUWBRange()
closest_distance_other_bot, other_id= self.RRT.getClosestRAB()
other_made_it = self.RRT.getMadeItID(other_id)
priority= True
if(other_id+1<own_id and other_made_it is not True):
priority = False
else:
priority = True
# print("own id + other_id+ priority",own_id,other_id,priority)
goal_angle_other = self.RRT.getGoalAngleID(other_id)
# print("priority bot", closest_distance_other_bot, own_id, other_id, priority )
#GRADIENTBUG: get both distance and id of closest bot
'''if (self.RRT.getClosestRAB()<100):
self.bot_is_close = True
print("bot is close!!!")
else:
self.bot_is_close = False'''
twist, self.rssi_goal_angle_adjust, self.goal_angle, gb_state = self.GB3.stateMachine(self.RRT.getRealDistanceToWall(),self.RRT.getRangeRight(),self.RRT.getRangeLeft(),
self.RRT.getHeading(), self.current_range, -1*rssi_noise, odometry, self.RRT.getArgosTime()/10,False, self.WF,self.RRT,outbound,
closest_distance_other_bot/100.0,priority,goal_angle_other)
gb_state_nr=0
if gb_state is "FORWARD":
gb_state_nr = 1
elif gb_state is "WALL_FOLLOWING":
gb_state_nr = 2
elif gb_state is "ROTATE_TO_GOAL":
gb_state_nr = 3
elif gb_state is "MOVE_OUT_OF_WAY":
gb_state_nr= 4
#numpy.savetxt('state_distance'+str(own_id)+'.txt',[gb_state_nr,closest_distance_other_bot] ,delimiter=',')
'''if closest_distance_other_bot/100.0<0.2:
twist = Twist()'''
# Call the controller from gradient_bug_v1 (gradient_bug repository)
'''
if outbound is False:
twist, self.rssi_goal_angle_adjust = self.GB.stateMachine(self.RRT.getRealDistanceToWall(),self.RRT.getRangeRight(),self.RRT.getRangeLeft(),
self.RRT.getHeading(),self.current_UWB_bearing, self.current_range, rssi_noise, odometry, self.RRT.getArgosTime()/10,False, self.WF,self.RRT,outbound)
else:
twist, self.rssi_goal_angle_adjust = self.GB2.stateMachine(self.RRT.getRealDistanceToWall(),self.RRT.getRangeRight(),self.RRT.getRangeLeft(),
self.RRT.getHeading(),self.current_UWB_bearing, self.current_range, rssi_noise, odometry, self.RRT.getArgosTime()/10,False, self.WF,self.RRT,outbound, self.bot_is_close)
'''
return twist, self.goal_angle, gb_state_nr, closest_distance_other_bot
# See if a value is within a margin from the wanted value
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
def wrap_pi(self, angles_rad):
return numpy.mod(angles_rad+numpy.pi, 2.0 * numpy.pi) - numpy.pi
class GradientBugController:
WF=wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0;
first_rotate = True
direction = 1
last_bearing = 0
hitpoint = PoseStamped()
stateStartTime=0
state = "ROTATE_TO_GOAL"
def __init__(self):
#Get Desired distance from the wall
self.distance_to_wall=self.WF.getWantedDistanceToWall();
self.direction = self.WF.getDirectionTurn();
self.hitpoint = PoseStamped();
self.first_rotate = True;
self.last_bearing = 0;
self.stateStartTime = 0;
self.state = "ROTATE_TO_GOAL"
self.pose_tower = PoseStamped();
self.first_run = 1
self.current_UWB_bearing = 2000
self.last_range = 0.0
self.current_range = 0.0
self.last_range_diff = 0.0
self.last_heading_rate = 2.5;
self.first_gradient = 1
self.diff_diff_range = 0.0
self.diff_range = 0.0
self.mem_range = 2000;
self.mem_heading = 1000
self.heading_before_turning = 0;
self.rotated_half_once = False
self.mem_hit_point_range = 2000
def stateMachine(self,RRT,odometry):
self.RRT = RRT
self.current_range = self.RRT.getUWBRange()
if self.first_gradient:
self.last_range = self.current_range
self.first_gradient =0
self.diff_range = (self.current_range-self.last_range)/100.0
self.diff_diff_range = self.last_range_diff-self.diff_range;
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front=self.RRT.getRangeFrontLeft()
range_side=self.RRT.getRangeLeft()
elif self.direction is -1:
range_front=self.RRT.getRangeFrontRight()
range_side=self.RRT.getRangeRight()
bot_pose = PoseStamped();
bot_pose.pose.position.x = odometry.pose.position.x;
bot_pose.pose.position.y = odometry.pose.position.y;
if self.first_run:
self.bot_init_position = self.RRT.getPoseBot();
pose_tower_abs = self.RRT.getPoseTower();
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x;
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y;
self.stateStartTime = deepcopy(self.RRT.getArgosTime())
if( abs(pose_tower_abs.pose.position.x)> 0.0):
self.first_run = 0
else:
rel_x = self.pose_tower.pose.position.x-bot_pose.pose.position.x ;
rel_y = self.pose_tower.pose.position.y - bot_pose.pose.position.y ;
theta = -1*self.RRT.getHeading();
rel_loc_x = rel_x*numpy.math.cos(theta)-rel_y*numpy.math.sin(theta)
rel_loc_y = rel_x*numpy.math.sin(theta)+rel_y*numpy.math.cos(theta)
self.current_UWB_bearing = numpy.arctan2(rel_loc_y,rel_loc_x)
#self.current_UWB_bearing = self.RRT.getUWBBearing();
print self.mem_heading
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1: #If an obstacle comes within the distance of the wall
self.mem_heading = self.RRT.getHeading()
if self.RRT.getAngleToWall()>0:
self.direction = -1
# self.last_heading_rate = 2.5;
else:
self.direction = 1
# self.last_heading_rate = -2.5;
if self.mem_hit_point_range < self.RRT.getUWBRange():
self.direction = -1*self.direction
# self.last_heading_rate = -1*self.last_heading_rate;
self.mem_hit_point_range = deepcopy(self.RRT.getUWBRange() +100)
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
#bot_pose = self.RRT.getPoseBot();
#If wall is lost by corner, rotate to goal again
if range_front>=2.0 and self.diff_range < 0 and\
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,0.05)!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,0.05)!=True)):
self.transition("ROTATE_TO_GOAL")
self.last_bearing = deepcopy(self.current_UWB_bearing)
self.WF.init()
if(self.RRT.getUWBRange()>self.mem_range and self.rotated_half_once == False):
self.transition("ROTATE_180")
self.WF.init()
self.direction = -1*self.direction
self.heading_before_turning = self.RRT.getHeading()
elif self.state=="ROTATE_TO_GOAL":
if self.logicIsCloseTo(self.diff_range,-0.035,0.0008) :
self.first_rotate = False
self.mem_range = deepcopy(self.RRT.getUWBRange())+200
self.transition("FORWARD")
if self.RRT.getRealDistanceToWall()<self.distance_to_wall+0.1:
self.transition("WALL_FOLLOWING")
self.first_rotate = False
elif self.state=="ROTATE_180":
if math.fabs(self.wrap_pi(self.RRT.getHeading()-self.heading_before_turning))>3.04:
self.rotated_half_once = True
self.transition("TURN_COMP")
elif self.state=="TURN_COMP":
if (self.RRT.getArgosTime() - self.stateStartTime)<2:
self.transition("WALL_FOLLOWING")
# Handle actions
if self.state == "FORWARD":
twist=self.WF.twistForward() #Go forward with maximum speed
#twist = self.headingGradientDescentRange()
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side,range_front,
self.RRT.getLowestValue(),self.RRT.getHeading(),self.RRT.getArgosTime(),self.direction)
elif self.state=="ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
#twist = self.headingGradientDescentRange()
if (self.RRT.getArgosTime() - self.stateStartTime)<self.WF.getDistanceAroundCorner90()/0.35 * 10:
twist=self.headingZigZag()
else:
twist = self.headingGradientDescentRange()
elif self.state=="ROTATE_180":
twist = self.WF.twistTurnInCorner(-self.direction)
elif self.state=="TURN_COMP":
twist = self.WF.twistTurnInCorner(-self.direction)
print self.state
#self.cmdVelPub.publish(twist)
self.lastTwist = twist
self.last_range_diff = deepcopy(self.diff_range)
self.last_range = deepcopy(self.current_range)
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
def headingGradientDescentRange(self):
v = 1
command = self.last_heading_rate
print(self.current_range,self.diff_range,self.diff_diff_range)
if(self.diff_diff_range>0):
command = -1*self.last_heading_rate
print('command',command)
# if abs(self.diff_diff_range)< 0.0005:
# print "zero command"
# command = self.diff_diff_range
#command = 0
#w = (command + self.last_heading_rate)/2
# command = 1000*self.diff_diff_range
#
# if abs(command)>2.5:
# command = 2.5 * command/abs(command)
#
w = command;
self.last_heading_rate = deepcopy(w)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def headingZigZag(self):
v = 1
command = -1*self.last_heading_rate
w = command;
self.last_heading_rate = deepcopy(w)
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
def wrap_pi(self, angles_rad):
return numpy.mod(angles_rad+numpy.pi, 2.0 * numpy.pi) - numpy.pi
class BugAlgorithms:
cmdVelPub = None
goalAnglePub = None
bug_type = "com_bug"
bug_controller = com_bug_controller.ComBugController()
RRT = receive_rostopics.RecieveROSTopic()
WF = wall_following.WallFollowing()
reset_bug = False
random_environment = False
odometry = [0, 0]
twist = Twist()
noise_level = 0.4
odometry = PoseStamped()
odometry_perfect = PoseStamped()
previous_time = 0
send_stop = False
outbound = True
outbound_angle = 0
start_time = 0
opened_file = False
pos_save = []
angle_goal = 0
# select the appropiate controller by the appropiate bug
def getController(self, argument):
switcher = {
"com_bug":
com_bug_controller.ComBugController(),
"bug_2":
bug_2_controller.Bug2Controller(),
"i_bug":
i_bug_controller.IBugController(),
"i_bug_2":
i_bug_2_controller.IBug2Controller(),
"alg_1":
alg_1_controller.Alg1Controller(),
"alg_2":
alg_2_controller.Alg2Controller(),
"wf":
wall_following_controller.WallFollowController(),
"blind_bug":
blind_bug_controller.BlindBugController(),
"gradient_bug":
gradient_bug_controller.GradientBugController(),
"gradient_embedded_bug":
gradient_bug_embedded_controller.GradientBugController(),
}
return switcher.get(argument, False)
def __init__(self):
# Subscribe to topics and init a publisher
self.cmdVelPub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.goalAnglePub = rospy.Publisher('goal_angle',
goal_angle,
queue_size=10)
self.madeItPub = rospy.Publisher('made_it', Bool, queue_size=10)
#rospy.Subscriber('proximity', ProximityList, self.RRT.prox_callback,queue_size=100)
#rospy.Subscriber('rangebearing', RangebearingList, self.RRT.rab_callback,queue_size=100)
#rospy.Subscriber('position', PoseStamped, self.RRT.pose_callback,queue_size=100)
rospy.Subscriber('position',
PoseStamped,
self.RRT.pose_callback,
queue_size=10)
rospy.Subscriber('/tower/position',
PoseStamped,
self.RRT.pose_callback_tower,
queue_size=10)
rospy.Subscriber('/switch_bug', String, self.switchBug, queue_size=10)
rospy.Subscriber('/random_environment',
Bool,
self.random_environment,
queue_size=10)
rospy.Subscriber('/noise_level',
Float64,
self.noise_level_cb,
queue_size=10)
rospy.Subscriber('RSSI_to_tower',
Float32,
self.RRT.rssi_tower_callback,
queue_size=10)
rospy.Subscriber("/bot1/goal_angle", goal_angle,
self.RRT.goal_angle_bot1_callback)
rospy.Subscriber("/bot2/goal_angle", goal_angle,
self.RRT.goal_angle_bot2_callback)
rospy.Subscriber("/bot3/goal_angle", goal_angle,
self.RRT.goal_angle_bot3_callback)
rospy.Subscriber("/bot4/goal_angle", goal_angle,
self.RRT.goal_angle_bot4_callback)
rospy.Subscriber("/bot5/goal_angle", goal_angle,
self.RRT.goal_angle_bot5_callback)
rospy.Subscriber("/bot6/goal_angle", goal_angle,
self.RRT.goal_angle_bot6_callback)
rospy.Subscriber("/bot7/goal_angle", goal_angle,
self.RRT.goal_angle_bot7_callback)
rospy.Subscriber("/bot8/goal_angle", goal_angle,
self.RRT.goal_angle_bot8_callback)
rospy.Subscriber("/bot9/goal_angle", goal_angle,
self.RRT.goal_angle_bot9_callback)
rospy.Subscriber("/bot1/made_it", Bool, self.RRT.made_it_bot1_callback)
rospy.Subscriber("/bot2/made_it", Bool, self.RRT.made_it_bot2_callback)
rospy.Subscriber("/bot3/made_it", Bool, self.RRT.made_it_bot3_callback)
rospy.Subscriber("/bot4/made_it", Bool, self.RRT.made_it_bot4_callback)
rospy.Subscriber("/bot5/made_it", Bool, self.RRT.made_it_bot5_callback)
rospy.Subscriber("/bot6/made_it", Bool, self.RRT.made_it_bot6_callback)
rospy.Subscriber("/bot7/made_it", Bool, self.RRT.made_it_bot7_callback)
rospy.Subscriber("/bot8/made_it", Bool, self.RRT.made_it_bot8_callback)
rospy.Subscriber("/bot9/made_it", Bool, self.RRT.made_it_bot9_callback)
'''
goal_angle_sub = []
topic_name = "/bot"+ str(it+1)+"/goal_angle"
goal_angle_sub_temp = message_filters.Subscriber(topic_name,goal_angle)
goal_angle_sub.append(goal_angle_sub_temp)
ts = message_filters.TimeSynchronizer([goal_angle_sub[0],goal_angle_sub[1]], 10)
ts.registerCallback(self.RRT.goal_angle_callback)
'''
#Wait for services to begin
rospy.wait_for_service('/start_sim')
s1 = rospy.Service('get_vel_cmd', GetCmds, self.runStateMachine)
try:
start_sim = rospy.ServiceProxy('/start_sim', StartSim)
# Start sim with indoor environment from file (from indoor environment generator package)
start_sim(4, 1, 1)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
self.bug_type = rospy.get_param('bug_algorithms/bug_type')
self.outbound_angle = rospy.get_param('bug_algorithms/outbound_angle')
self.bug_controller = self.getController(self.bug_type)
self.bug_controller.__init__()
if self.bug_controller == False:
print "Wrong bug type!"
self.send_stop = False
# Ros loop were the rate of the controller is handled
def rosLoop(self):
rospy.spin()
def switchBug(self, req):
print("SWITCH BUG")
self.bug_controller = self.getController(req.data)
self.bug_type = req.data
self.reset_bug = True
self.opened_file = False
print req.data
try:
start_sim = rospy.ServiceProxy('/start_sim', StartSim)
if (self.random_environment):
#start_sim(1,1,1)
start_sim(4, 1, 1)
self.random_environment = False
print "python, send regenerate environment"
else:
start_sim(2, 1, 1)
print "python, reset experiment with same environment"
except rospy.ServiceException as e:
print "Service call failed: %s" % e
if self.bug_controller == False:
print "Wrong bug type!"
def runStateMachine(self, req):
rospy.wait_for_service('/stop_sim')
try:
stop_sim = rospy.ServiceProxy('/stop_sim', Empty)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
#Send values from service to the rostopic retrieval
self.RRT.prox_callback(req.proxList)
self.RRT.rab_callback(req.RabList)
self.RRT.pose_callback(req.PosQuat)
# Get the current position of the bot
pose_bot = PoseStamped()
pose_bot = self.RRT.getPoseBot()
# Get the distance to the tower
distance_to_tower = math.sqrt(
math.pow(pose_bot.pose.position.x, 2) +
math.pow(pose_bot.pose.position.y, 2))
angle_to_tower = math.atan2(pose_bot.pose.position.y,
pose_bot.pose.position.x)
number_id = int(filter(str.isdigit, req.botID.data))
# print self.RRT.getArgosTime()/10
# If the bug algorithm is reset or the bug has been changed, initialize everythong
if req.reset or self.reset_bug:
self.WF.init()
self.bug_controller.__init__()
self.reset_bug = False
self.odometry = PoseStamped()
self.odometry_perfect = PoseStamped()
self.send_stop = False
self.outbound = True
self.start_time = self.RRT.getArgosTime()
if req.reset or self.opened_file is False:
self.F = open("trajectory" + str(number_id) + ".txt", "w")
self.F_state = open("state_distance" + str(number_id) + ".txt",
"w")
self.opened_file = True
#Get time and change outbound to back after 3 min
if (self.RRT.getArgosTime() -
self.start_time) / 10 > 300 + number_id * 10:
self.outbound = False
if self.RRT.getArgosTime() % 1000 is 0:
print(self.RRT.getArgosTime() - self.start_time)
# If the bug is not near the tower (yet)
#if self.RRT.getArgosTime()%100 is 0:
# print(distance_to_tower)
if ((distance_to_tower > 2.5 or self.outbound == True)):
self.F.write(
"%5.2f, %5.2f\n" %
(req.PosQuat.pose.position.x, req.PosQuat.pose.position.y))
# self.pos_save.append(req.PosQuat.pose.position.x)
# self.pos_save.append(req.PosQuat.pose.position.y)
# self.pos_save.append([])
#Select the bug statemachine based on the launch file
gb_state_nr = 0
closest_distance_other_bot = 0
if self.bug_type == 'alg_1':
self.twist = self.bug_controller.stateMachine(
self.RRT, self.get_odometry_from_commands(0.0), 0.0,
self.noise_level)
elif self.bug_type == 'alg_2':
self.twist = self.bug_controller.stateMachine(
self.RRT, self.get_odometry_from_commands(0.0), 0.0,
self.noise_level, 0.0)
elif self.bug_type == 'i_bug':
self.twist = self.bug_controller.stateMachine(
self.RRT, self.get_odometry_from_commands(0.0),
self.noise_level)
else:
if self.RRT.getArgosTime() / 10 < number_id * 10:
self.twist = Twist()
else:
self.twist, self.angle_goal, gb_state_nr, closest_distance_other_bot = self.bug_controller.stateMachine(
self.RRT,
self.get_odometry_from_commands(self.noise_level),
self.outbound, self.outbound_angle, number_id)
self.F_state.write("%d, %5.2f\n" %
(gb_state_nr, closest_distance_other_bot))
#Save values for the debugging (matlab)
# Odometry with drift
# numpy.savetxt('rel_x.txt',[self.odometry.pose.position.x],delimiter=',')
# numpy.savetxt('rel_y.txt',[self.odometry.pose.position.y],delimiter=',')
# Odometry perfect
# self.get_odometry_from_commands_perfect()
# numpy.savetxt('rel_x_per.txt',[self.odometry_perfect.pose.position.x],delimiter=',')
# numpy.savetxt('rel_y_per.txt',[self.odometry_perfect.pose.position.y],delimiter=',')
msg_temp = goal_angle()
msg_temp.goal_angle.data = self.angle_goal
msg_temp.header.stamp = rospy.Time.now()
self.goalAnglePub.publish(msg_temp)
#Return the commands to the controller
msg_madeit = Bool()
msg_madeit.data = False
self.madeItPub.publish(msg_madeit)
return GetCmdsResponse(self.twist)
else:
#Stop the bug and sent a stop signal for the simulator
if (distance_to_tower < 0.5):
self.twist.linear.x = 0
self.twist.angular.z = 0
else:
self.twist = self.go_to_tower(angle_to_tower)
#self.F = open("trajectory"+str(number_id)+".txt","w")
# self.F.close()
msg_madeit = Bool()
msg_madeit.data = True
self.madeItPub.publish(msg_madeit)
if (self.send_stop is False):
# numpy.savetxt("trajectory"+str(number_id)+".txt",self.pos_save,delimiter=',')
print "bug has reached goal"
stop_sim()
self.send_stop = True
self.outbound = True
self.F.close()
return GetCmdsResponse(self.twist)
# Make a (noisy) odometry measurment based on the inputs on the system, based on a guassian, returns an odometry position.
def get_odometry_from_commands(self, noise):
current_time = float(self.RRT.getArgosTime()) / 10
diff_time = current_time - self.previous_time
if noise < 0.01:
noisy_velocity_estimate = self.twist.linear.x * 0.35
noisy_heading = self.RRT.getHeading()
else:
noisy_velocity_estimate = numpy.random.normal(
self.twist.linear.x * 0.35, noise, 1)
noisy_heading = numpy.random.normal(self.RRT.getHeading(), noise,
1)
self.odometry.pose.position.x = self.odometry.pose.position.x + diff_time * noisy_velocity_estimate * math.cos(
noisy_heading)
self.odometry.pose.position.y = self.odometry.pose.position.y + diff_time * noisy_velocity_estimate * math.sin(
noisy_heading)
self.previous_time = current_time
return self.odometry
# Returns a perfect odometry, for comparison purposes
def get_odometry_from_commands_perfect(self):
noisy_velocity_estimate = self.twist.linear.x * 0.035
self.odometry_perfect.pose.position.x = self.odometry_perfect.pose.position.x + noisy_velocity_estimate * math.cos(
self.RRT.getHeading())
self.odometry_perfect.pose.position.y = self.odometry_perfect.pose.position.y + noisy_velocity_estimate * math.sin(
self.RRT.getHeading())
return self.odometry
# Retrieve command if need to make another environment
def random_environment(self, req):
self.random_environment = req.data
# Retrieve noise level from topic
def noise_level_cb(self, req):
self.noise_level = req.data
def go_to_tower(self, angle_tower):
twist = Twist()
if self.logicIsCloseTo(
self.wrap_pi(3.14 - (self.RRT.getHeading() - angle_tower)), 0,
0.1):
twist.linear.x = 0.5
twist.angular.z = 0
else:
twist.linear.x = 0
twist.angular.z = 1.0
return twist
# See if a value is within a margin from the wanted value
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
def wrap_pi(self, angles_rad):
return numpy.mod(angles_rad + numpy.pi, 2.0 * numpy.pi) - numpy.pi
class IBugController:
state = "ROTATE_TO_GOAL"
stateStartTime = 0
WF = wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0
previous_leave_point = 1000.0
previous_hit_point = 1000.0
first_rotate = True
direction = 1
hitpoint = PoseStamped()
last_bearing = 0
# Constants
MAX_FORWARD_SPEED = 1
MAX_ROTATION_SPEED = 2.5
def __init__(self):
#Get Desired distance from the wall
self.distance_to_wall = self.WF.getWantedDistanceToWall()
self.direction = self.WF.getDirectionTurn()
self.hitpoint = PoseStamped()
self.last_bearing = 0
self.stateStartTime = 0
self.first_rotate = True
self.previous_leave_point = 1000.0
self.previous_hit_point = 1000.0
self.heading_before_turning = 0
self.state = "ROTATE_TO_GOAL"
self.pose_tower = PoseStamped()
self.first_run = 1
self.current_UWB_bearing = 2000
self.current_UWB_range = 1000
def stateMachine(self, RRT, odometry, range_noise):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front = self.RRT.getRangeFrontLeft()
range_side = self.RRT.getRangeLeft()
elif self.direction is -1:
range_front = self.RRT.getRangeFrontRight()
range_side = self.RRT.getRangeRight()
# Rotation to goal based on odometery
bot_pose = PoseStamped()
bot_pose.pose.position.x = odometry.pose.position.x
bot_pose.pose.position.y = odometry.pose.position.y
if self.first_run:
self.bot_init_position = self.RRT.getPoseBot()
pose_tower_abs = self.RRT.getPoseTower()
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y
if (abs(pose_tower_abs.pose.position.x) > 0.0):
self.first_run = 0
else:
rel_x = self.pose_tower.pose.position.x - bot_pose.pose.position.x
rel_y = self.pose_tower.pose.position.y - bot_pose.pose.position.y
theta = -1 * self.RRT.getHeading()
rel_loc_x = rel_x * numpy.math.cos(theta) - rel_y * numpy.math.sin(
theta)
rel_loc_y = rel_x * numpy.math.sin(theta) + rel_y * numpy.math.cos(
theta)
self.current_UWB_bearing = numpy.arctan2(rel_loc_y, rel_loc_x)
self.current_UWB_range = math.sqrt(rel_loc_x**2 + rel_loc_y**2)
self.current_UWB_range = numpy.random.normal(
self.current_UWB_range, range_noise, 1)
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall(
) < self.distance_to_wall + 0.1: #If an obstacle comes within the distance of the wall
self.previous_hit_point = self.current_UWB_range
self.hitpoint.pose.position.x = odometry.pose.position.x
self.hitpoint.pose.position.y = odometry.pose.position.y
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
bot_pose = PoseStamped()
bot_pose.pose.position.x = odometry.pose.position.x
bot_pose.pose.position.y = odometry.pose.position.y
#If wall is lost by corner, rotate to goal again
if range_front>=2.0 and self.current_UWB_range<self.previous_hit_point and \
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,0.05)!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,0.05)!=True)):
self.transition("ROTATE_TO_GOAL")
self.last_bearing = self.current_UWB_bearing
self.WF.init()
elif self.state == "ROTATE_TO_GOAL":
self.previous_leave_point = self.current_UWB_range
if self.logicIsCloseTo(0, self.current_UWB_bearing, 0.05):
self.first_rotate = False
self.transition("FORWARD")
if self.RRT.getRealDistanceToWall() < self.distance_to_wall + 0.1:
self.transition("WALL_FOLLOWING")
self.first_rotate = False
# Handle actions
if self.state == "FORWARD":
twist = self.WF.twistForward() #Go forward with maximum speed
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side, range_front,
self.RRT.getLowestValue(),
self.RRT.getHeading(),
self.RRT.getArgosTime(),
self.direction)
elif self.state == "ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
if self.first_rotate or\
(self.last_bearing<0 and self.direction == 1) or\
(self.last_bearing>0 and self.direction == -1):
twist = self.WF.twistTurnInCorner(self.direction)
else:
if (self.RRT.getArgosTime() - self.stateStartTime) < 20:
twist = self.WF.twistForward()
else:
twist = self.WF.twistTurnAroundCorner(
self.distance_to_wall + 0.4, self.direction)
print self.state
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
def twistRotateToGoal(self):
v = 0
w = self.MAX_ROTATION_SPEED * numpy.sign(self.RRT.getUWBBearing())
twist = Twist()
twist.linear.x = v
twist.angular.z = w
return twist
class Alg2Controller:
state = "ROTATE_TO_GOAL"
stateStartTime = 0
WF = wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0
first_rotate = True
hitpoint = PoseStamped()
heading_before_turning = 0
hit_points = []
direction = 1
init_direction = 1
last_bearing = 0
rotated_half_once = False
def __init__(self):
self.distance_to_wall = self.WF.getWantedDistanceToWall()
# FIX THIS HACK!
self.direction = 1
#self.WF.getDirectionTurn();
self.init_direction = 1
#self.WF.getDirectionTurn();
self.hitpoint = PoseStamped()
self.bot_tower_slope = 0
self.hit_points = []
self.last_bearing = 0
self.stateStartTime = 0
self.first_rotate = True
self.heading_before_turning = 0
self.state = "ROTATE_TO_GOAL"
self.previous_hit_point = 1000.0
self.pose_tower = PoseStamped()
self.first_run = 1
self.current_UWB_bearing = 2000
self.current_UWB_range = 1000
self.rand_FN = 0
# Ros loop were the rate of the controller is handled
def rosLoop(self):
rate = rospy.Rate(30)
while not rospy.is_shutdown():
self.stateMachine()
rate.sleep()
def stateMachine(self, RRT, odometry, falsepositive_ratio,
falsenegative_ratio, range_noise):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front = self.RRT.getRangeFrontLeft()
range_side = self.RRT.getRangeLeft()
elif self.direction is -1:
range_front = self.RRT.getRangeFrontRight()
range_side = self.RRT.getRangeRight()
# Rotation to goal based on odometery
bot_pose = PoseStamped()
bot_pose.pose.position.x = odometry.pose.position.x
bot_pose.pose.position.y = odometry.pose.position.y
if self.first_run:
self.bot_init_position = self.RRT.getPoseBot()
pose_tower_abs = self.RRT.getPoseTower()
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y
if (abs(pose_tower_abs.pose.position.x) > 0.0):
self.first_run = 0
else:
rel_x = self.pose_tower.pose.position.x - bot_pose.pose.position.x
rel_y = self.pose_tower.pose.position.y - bot_pose.pose.position.y
theta = -1 * self.RRT.getHeading()
rel_loc_x = rel_x * numpy.math.cos(theta) - rel_y * numpy.math.sin(
theta)
rel_loc_y = rel_x * numpy.math.sin(theta) + rel_y * numpy.math.cos(
theta)
self.current_UWB_bearing = numpy.arctan2(rel_loc_y, rel_loc_x)
self.current_UWB_range = math.sqrt(rel_loc_x**2 + rel_loc_y**2)
self.current_UWB_range = numpy.random.normal(
self.current_UWB_range, range_noise, 1)
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall(
) < self.distance_to_wall + 0.1: #If an obstacle comes within the distance of the wall
#self.hitpoint = self.RRT.getPoseBot();
self.hitpoint.pose.position.x = odometry.pose.position.x
self.hitpoint.pose.position.y = odometry.pose.position.y
self.previous_hit_point = self.current_UWB_range
rand_FP = random.random()
self.rand_FN = random.random()
if ((self.checkHitPoints(self.hitpoint)
and self.rand_FN > falsenegative_ratio)
or rand_FP < falsepositive_ratio):
print "already hit point!"
self.rotated_half_once = True
self.direction = -1 * self.direction
else:
print "Did not hit point"
self.rand_FN = random.random()
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
rand_FP = random.random()
if ((self.checkHitPoints(bot_pose) and self.rand_FN>falsenegative_ratio) or rand_FP<falsepositive_ratio) and self.rotated_half_once == False and \
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,self.WF.getLocationPrecision())!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,self.WF.getLocationPrecision())!=True)):
self.transition("ROTATE_180")
self.WF.init()
self.direction = -1 * self.direction
self.heading_before_turning = self.RRT.getHeading()
if range_front>=2.0 and self.current_UWB_range<self.previous_hit_point and\
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,0.05)!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,0.05)!=True)):
self.transition("ROTATE_TO_GOAL")
self.last_bearing = self.current_UWB_bearing
self.WF.init()
self.hit_points.append(deepcopy(self.hitpoint))
print "saved hitpoint"
elif self.state == "ROTATE_TO_GOAL":
if self.logicIsCloseTo(0, self.current_UWB_bearing, 0.05):
self.first_rotate = False
self.rotated_half_once = False
self.direction = self.init_direction
self.transition("FORWARD")
else:
if self.RRT.getRealDistanceToWall(
) < self.distance_to_wall + 0.1:
self.transition("WALL_FOLLOWING")
self.first_rotate = False
elif self.state == "ROTATE_180":
if math.fabs(
self.wrap_pi(self.RRT.getHeading() -
self.heading_before_turning)) > 3.04:
self.rotated_half_once = True
self.transition("TURN_COMP")
elif self.state == "TURN_COMP":
if (self.RRT.getArgosTime() - self.stateStartTime) < 2:
self.transition("WALL_FOLLOWING")
if self.direction is 1:
range_front = self.RRT.getRangeFrontLeft()
range_side = self.RRT.getRangeLeft()
elif self.direction is -1:
range_front = self.RRT.getRangeFrontRight()
range_side = self.RRT.getRangeRight()
# Handle actions
if self.state == "FORWARD":
twist = self.WF.twistForward() #Go forward with maximum speed
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side, range_front,
self.RRT.getLowestValue(),
self.RRT.getHeading(),
self.RRT.getArgosTime(),
self.direction)
elif self.state == "ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
if self.first_rotate or\
(self.last_bearing<0 and self.direction == 1) or\
(self.last_bearing>0 and self.direction == -1):
twist = self.WF.twistTurnInCorner(self.direction)
else:
if (self.RRT.getArgosTime() - self.stateStartTime
) < self.WF.getDistanceAroundCorner90() / 0.35 * 10:
twist = self.WF.twistForward()
else:
twist = self.WF.twistTurnAroundCorner(
self.distance_to_wall + 0.3, self.direction)
elif self.state == "ROTATE_180":
twist = self.WF.twistTurnInCorner(-self.direction)
elif self.state == "TURN_COMP":
twist = self.WF.twistTurnInCorner(-self.direction)
print self.state
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
def checkHitPoints(self, bot_pose):
for i in range(0, len(self.hit_points)):
if ((self.logicIsCloseTo(self.hit_points[i].pose.position.x, bot_pose.pose.position.x,self.WF.getLocationPrecision())==True ) and \
(self.logicIsCloseTo(self.hit_points[i].pose.position.y, bot_pose.pose.position.y,self.WF.getLocationPrecision())==True)):
return True
return False
def wrap_pi(self, angles_rad):
return numpy.mod(angles_rad + numpy.pi, 2.0 * numpy.pi) - numpy.pi
class WallFollowController:
state = "FORWARD"
cmdVelPub = None
puckList = None
WF = wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0
direction = 1
def __init__(self):
self.distance_to_wall = self.WF.getWantedDistanceToWall()
self.direction = self.WF.getDirectionTurn()
self.state = "FORWARD"
def rosLoop(self):
rate = rospy.Rate(100)
rospy.sleep(4)
while not rospy.is_shutdown():
self.stateMachine()
rate.sleep()
def stateMachine(self, RRT, odometry):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front = self.RRT.getRangeFrontLeft()
range_side = self.RRT.getRangeLeft()
elif self.direction is -1:
range_front = self.RRT.getRangeFrontRight()
range_side = self.RRT.getRangeRight()
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getLowestValue(
) < self.distance_to_wall + 0.1 and self.RRT.getLowestValue(
) != 0.0:
self.transition("WALL_FOLLOWING")
elif self.state == "ROTATE_TO_GOAL":
print self.RRT.getUWBBearing()
if self.logicIsCloseTo(0, self.RRT.getUWBBearing(), 0.1):
self.first_rotate = False
self.transition("FORWARD")
# Handle actions
if self.state == "FORWARD":
twist = self.WF.twistForward()
elif self.state == "WALL_FOLLOWING":
twist = self.WF.wallFollowingController(range_side, range_front,
self.RRT.getLowestValue(),
self.RRT.getHeading(),
self.RRT.getArgosTime(),
self.direction)
elif self.state == "ROTATE_TO_GOAL":
twist = self.WF.twistTurnInCorner(self.direction)
print self.state
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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
class Bug2Controller:
state = "ROTATE_TO_GOAL"
stateStartTime=0
WF=wall_following.WallFollowing()
RRT = receive_rostopics.RecieveROSTopic()
distance_to_wall = 0;
bot_init_position = PoseStamped()
pose_tower = PoseStamped()
bot_tower_slope = 0;
hitpoint = PoseStamped()
direction = 1
first_run = 1
obstacle_is_hit = 0
def __init__(self):
self.distance_to_wall=self.WF.getWantedDistanceToWall();
self.direction = self.WF.getDirectionTurn();
self.hitpoint = PoseStamped();
self.bot_init_position = PoseStamped()
self.first_rotate = True;
self.last_bearing = 0;
self.stateStartTime = 0;
self.obstacle_is_hit = 0;
self.state = "ROTATE_TO_GOAL"
self.bot_tower_slope = 0
self.previous_leave_point =1000.0;
self.pose_tower = PoseStamped();
self.first_run = 1
self.current_UWB_bearing = 2000
self.current_UWB_range = 1000
def stateMachine(self,RRT,odometry):
self.RRT = RRT
range_front = 1000.0
range_side = 1000.0
if self.direction is 1:
range_front=self.RRT.getRangeFrontLeft()
range_side=self.RRT.getRangeLeft()
elif self.direction is -1:
range_front=self.RRT.getRangeFrontRight()
range_side=self.RRT.getRangeRight()
bot_tower_slope_run = 0;
bot_pose = PoseStamped();
bot_pose.pose.position.x = odometry.pose.position.x;
bot_pose.pose.position.y = odometry.pose.position.y;
if self.first_run:
self.bot_init_position = self.RRT.getPoseBot();
pose_tower_abs = self.RRT.getPoseTower();
self.pose_tower.pose.position.x = pose_tower_abs.pose.position.x - self.bot_init_position.pose.position.x;
self.pose_tower.pose.position.y = pose_tower_abs.pose.position.y - self.bot_init_position.pose.position.y;
if math.fabs(self.pose_tower.pose.position.x -self.bot_init_position.pose.position.x)>0:
self.bot_tower_slope = (self.pose_tower.pose.position.y -self.bot_init_position.pose.position.y)/(self.pose_tower.pose.position.x -self.bot_init_position.pose.position.x);
self.first_run = 0
else:
#bot_pose = self.RRT.getPoseBot();
#self.pose_tower= self.RRT.getPoseTower();
bot_tower_slope_run = (self.pose_tower.pose.position.y -bot_pose.pose.position.y)/(self.pose_tower.pose.position.x -bot_pose.pose.position.x);
rel_x = self.pose_tower.pose.position.x-bot_pose.pose.position.x ;
rel_y = self.pose_tower.pose.position.y - bot_pose.pose.position.y ;
theta = -1*self.RRT.getHeading();
rel_loc_x = rel_x*numpy.math.cos(theta)-rel_y*numpy.math.sin(theta)
rel_loc_y = rel_x*numpy.math.sin(theta)+rel_y*numpy.math.cos(theta)
self.current_UWB_bearing = numpy.arctan2(rel_loc_y,rel_loc_x)
self.current_UWB_range = math.sqrt(rel_loc_x**2 +rel_loc_y**2)
# Handle State transition
if self.state == "FORWARD":
if self.RRT.getRealDistanceToWall()<self.distance_to_wall: #If an obstacle comes within the distance of the wall
# self.hitpoint = self.RRT.getPoseBot();
self.hitpoint.pose.position.x = odometry.pose.position.x;
self.hitpoint.pose.position.y = odometry.pose.position.y;
self.previous_hit_point = self.current_UWB_range;
self.transition("WALL_FOLLOWING")
elif self.state == "WALL_FOLLOWING":
bot_pose.pose.position.x = odometry.pose.position.x;
bot_pose.pose.position.y = odometry.pose.position.y;
if self.logicIsCloseTo(self.bot_tower_slope, bot_tower_slope_run,0.1) and\
rel_x>0 and self.current_UWB_range<self.previous_hit_point and\
((self.logicIsCloseTo(self.hitpoint.pose.position.x, bot_pose.pose.position.x,self.WF.getLocationPrecision())!=True ) or \
(self.logicIsCloseTo(self.hitpoint.pose.position.y, bot_pose.pose.position.y,self.WF.getLocationPrecision())!=True)):
self.transition("ROTATE_TO_GOAL")
self.last_bearing = self.current_UWB_bearing
self.WF.init()
elif self.state=="ROTATE_TO_GOAL":
if self.logicIsCloseTo(0,self.current_UWB_bearing,0.05) :
self.transition("FORWARD")
# Handle actions
if self.state == "FORWARD":
twist=self.WF.twistForward() #Go forward with maximum speed
elif self.state == "WALL_FOLLOWING":
# Wall following controller of wall_following.py
twist = self.WF.wallFollowingController(range_side,range_front,
self.RRT.getLowestValue(),self.RRT.getHeading(),self.RRT.getArgosTime(),self.direction)
elif self.state=="ROTATE_TO_GOAL":
#First go forward for 2 seconds (to get past any corner, and then turn
if self.last_bearing>0:
twist = self.WF.twistTurnInCorner(-1)
else:
twist = self.WF.twistTurnInCorner(1)
print self.state
self.lastTwist = twist
return twist
# Transition state and restart the timer
def transition(self, newState):
self.state = newState
self.stateStartTime = self.RRT.getArgosTime()
# See if a value is within a margin from the wanted value
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