class CleaningRobot: def __init__(self, webcontroller, photographer, cleaner_side="right"): print("Initializing CleaningRobot") self.webcontroller = webcontroller self.photographer = photographer self.cleaner_side = cleaner_side self.FOLLOW_WALL_SET_POINT = 0.40 self.FOLLOW_WALL_SET_POINT_START = 0.35 self.FOLLOW_WALL_SET_POINT_END = 0.50 self.OBSTACLE_DETECTION_SET_POINT_FAR = 1.00 self.STUCK_DETECTION_DISTANCE = 0.20 self.STUCK_MAX_COUNTER = 30 self.PID_controller = PIDController(self.FOLLOW_WALL_SET_POINT) self.current_speed = Twist() self.cur_command = "start" self.previous_searching_time = 0 self.state = 0 self.previous_state = 0 self.robot_stuck_counter = 0 self.state_desc = { 0: 'Idle', 1: 'Finding wall', 2: 'Following wall', 3: 'Stuck', 4: 'Turning' } self.directions = { 'left':10, 'fleft':10, 'front1':10, 'front2':10, 'front':10, 'fright':10, 'right':10, } self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size =1 ) self.subscriber = rospy.Subscriber('/scan',LaserScan, self.callback_laser_scan) def _sendStatusMessage(self, msg): self.webcontroller.SendStatus(msg) def callback_laser_scan(self,msg): self.directions = { 'right':min(min(msg.ranges[270:305]),10), 'fright':min(min(msg.ranges[306:341]),10), 'front1':min(min(msg.ranges[342:360]),10), 'front2':min(min(msg.ranges[0:17]),10), 'front':min(min(min(msg.ranges[342:360]),min(msg.ranges[0:17])),10), 'fleft':min(min(msg.ranges[18:53]),10), 'left':min(min(msg.ranges[54:90]),10), } def _wall_at_right(self,distance_set_point): distance = min(self.directions['right'], self.directions['fright']) return distance - distance_set_point <= 0 def _wall_at_front(self,distance_set_point): distance = self.directions['front'] return distance - distance_set_point <= 0 def _wall_at_left(self,distance_set_point): distance = min(self.directions['left'], self.directions['fleft']) return distance - distance_set_point <= 0 def wall_is_found(self,distance_set_point): return self._wall_at_right(distance_set_point) or self._wall_at_front(distance_set_point) or self._wall_at_left(distance_set_point) def _is_in_corner(self): if self.cleaner_side == "right": if self.directions["right"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["fright"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["front1"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["front2"] < self.FOLLOW_WALL_SET_POINT\ and not self.directions["left"] < self.FOLLOW_WALL_SET_POINT: return True if self.cleaner_side == "left": if self.directions["left"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["fleft"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["front1"] < self.FOLLOW_WALL_SET_POINT\ and self.directions["front2"] < self.FOLLOW_WALL_SET_POINT\ and not self.directions["right"] < self.FOLLOW_WALL_SET_POINT: return True return False def robot_is_stuck(self): minimum_distance = min(self.directions.values()) print '\nstuck counter: %f , minimun distance: %f ' %(self.robot_stuck_counter,minimum_distance) if minimum_distance <= self.STUCK_DETECTION_DISTANCE: self.robot_stuck_counter += 1 else: self.robot_stuck_counter = 0 if self.robot_stuck_counter >= self.STUCK_MAX_COUNTER: return True return False def vortex_searching(self,increment_delay,search_speed_increment,search_speed_max): current_time = time() if current_time - self.previous_searching_time >= increment_delay: self.current_speed.linear.x += search_speed_increment self.previous_searching_time = current_time self.current_speed.angular.z = 0.2 self.current_speed.linear.x = min(self.current_speed.linear.x,search_speed_max) def finding_wall(self,distance_set_point): #detecting wall position #act based on the position """ if self._wall_at_front(distance_set_point): #at far front, approaching slowly self.current_speed.linear.x = 0.1 self.current_speed.angular.z = 0 elif self._wall_at_right(distance_set_point): #at far right, rotate right self.current_speed.linear.x = 0 self.current_speed.angular.z = -0.1 elif self._wall_at_left(distance_set_point): #at far left, rotate left self.current_speed.linear.x = 0 self.current_speed.angular.z = 0.1 """ if self._is_in_corner(): self.state = STATES["TURNING"] else: #no obstacles, vortex searching self.vortex_searching(10,0.05,1.0) def state_work(self): if self.cur_command == "stop": print '\nstop command received, to idle' self.current_speed.linear.x = 0 self.current_speed.angular.z = 0 self.publisher.publish(self.current_speed) self.state = 0 self._sendStatusMessage(self.state_desc[self.state]) #idle state if self.state == STATES["IDLE"]: if self.cur_command == "start": #start finding wall print '\nstart command received to find wall' self.state = STATES["FIND_WALL"] self._sendStatusMessage(self.state_desc[self.state]) #find wall state elif self.state == STATES["FIND_WALL"]: if self.robot_is_stuck(): print '\nrobot is stuck, to idle' self.current_speed.linear.x = 0 self.current_speed.angular.z = 0 self.publisher.publish(self.current_speed) self.cur_command = "stop" self.state = STATES["STUCK"] self._sendStatusMessage(self.state_desc[self.state]) self.photographer.CaptureImage() elif self.wall_is_found(self.FOLLOW_WALL_SET_POINT_START): #change to follow wall print '\nto follow wall' self.state = STATES["FOLLOW_WALL"] self._sendStatusMessage(self.state_desc[self.state]) else: print '\nfinding wall' self.finding_wall(self.OBSTACLE_DETECTION_SET_POINT_FAR) self.publisher.publish(self.current_speed) #print '\nfleft: %f ,front: %f ,fright: %f ' %(self.directions['front'], self.directions['fleft'], self.directions['fright']) #follow wall state elif self.state == STATES["FOLLOW_WALL"]: if self.robot_is_stuck(): print '\nrobot is stuck, to idle' self.current_speed.linear.x = 0 self.current_speed.angular.z = 0 self.cur_command = "stop" self.publisher.publish(self.current_speed) self.state = STATES["STUCK"] self._sendStatusMessage(self.state_desc[self.state]) self.photographer.CaptureImage() elif self._is_in_corner(): print "In a corner" self.state = STATES["TURNING"] self.current_speed.linear.x = 0 self.current_speed.angular.z = 1 if self.cleaner_side == "right" else -1 self.publisher.publish(self.current_speed) self._sendStatusMessage(self.state_desc[self.state]) else: print '\nfollowing wall' if self.cleaner_side == "right": distance_to_wall = min(self.directions['right'],self.directions['fright'],self.directions['front1']) elif self.cleaner_side == "left": distance_to_wall = min(self.directions['left'],self.directions['fleft'],self.directions['front2']) self.current_speed = self.PID_controller.GetPV(distance_to_wall) if self.cleaner_side == "left": self.current_speed.angular.z = self.current_speed.angular.z * -1 self.publisher.publish(self.current_speed) elif self.state == STATES["STUCK"]: if self.cur_command == "start": state = STATES["IDLE"] self._sendStatusMessage(self.state_desc[self.state]) elif self.state == STATES["TURNING"]: if not self._is_in_corner(): self.state = STATES["FOLLOW_WALL"] self._sendStatusMessage(self.state_desc[self.state]) else: self.current_speed.linear.x = 0 self.current_speed.angular.z = 0 self.publisher.publish(self.current_speed) self.state = STATES["IDLE"] print '\nunknown state, to idle' self._sendStatusMessage(self.state_desc[self.state]) print '\nleft: %f ,front: %f ,right: %f ' %(self.directions['left'],self.directions['front'],self.directions['right']) print '\nlinear: %f ,angular: %f ' %(self.current_speed.linear.x, self.current_speed.angular.z )