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