class Controller: '''Driver class for controlling tour guide's hybrid paradigm''' def __init__(self): # Initialize ROS node rospy.init_node("Controller") self.name = "Controller object" self.rate = rospy.Rate(10) # allow node to 'spin' at 10hz rospy.on_shutdown(self.shutdown) # calls this function when Ctrl+C self.motion = Motion() self.sensor = Sensor() self.localizer = Localizer() self.visited_highlights = [] # Boolean for reached goal self.goalReached = False def start(self): rospy.loginfo("Starting Tour Guide..") begin = 'q' # Note: the Motion, Sensor, and Localizer classes # seem to take a second to fully initialize so we give # this Controller a second to fully initialize as well rospy.sleep(1) # TODO: here, could we determine if the guide will be # 1) casually walking around, # 2) begin P2P mode, or # 3) begin tour mode? # TODO: for now, it will be 'casually walking around' # TODO: Chris - I was thinking we should just take out 'idle_mode' # for demo purposes. As soon as the program starts, we can ask the user # if they would like a P2P or a regular tour # then go from there # self.idle_mode() # TODO: Chris - I think it will also be easier if we just have the robot # start the tour at its initial position which is at the West Entrance rospy.loginfo("Would you like a P2P tour or a regular tour?") rospy.loginfo("Press a key:") rospy.loginfo("'1': Point To Point") rospy.loginfo("'2': Regular Tour") begin = input() if (begin == 1): self.point_to_point_mode() elif (begin == 2): self.tour_guide_mode() def idle_mode(self): '''Make Turtlebot move around aimlessly and avoid nearby obstacles ''' rospy.loginfo("Initalizing Idle mode.") while not rospy.is_shutdown(): if self.sensor.is_obstacle_detected(): # Rotate Turtlebot accordingly to avoid obstacles self.sensor.avoid_obstacle() else: # If no obstacles detected, no need to rotate self.motion.halt_rotation() self.motion.move_forward() def tour_guide_mode(self): ''' Robot will decide its path/highlights depending on its location''' rospy.loginfo("Initializing Tour Guide mode.") print(self.localizer.position) west_entrance = Locations.WestEntrance east_entrance = Locations.EastEntrance # TODO: just start at nearest entrance and go down the list if self.get_nearest_entrance(west_entrance, east_entrance) == west_entrance: self.localizer.moveToGoal(Locations.WestEntrance) self.localizer.moveToGoal(Locations.Atrium) self.localizer.moveToGoal(Locations.CSOffice) self.localizer.moveToGoal(Locations.ElectronicsLab) self.localizer.moveToGoal(Locations.ComputerLab) self.localizer.moveToGoal(Locations.EastEntrance) else: self.localizer.moveToGoal(Locations.EastEntrance) self.localizer.moveToGoal(Locations.ComputerLab) self.localizer.moveToGoal(Locations.ElectronicsLab) self.localizer.moveToGoal(Locations.Atrium) self.localizer.moveToGoal(Locations.CSOffice) self.localizer.moveToGoal(Locations.WestEntrance) ''' # Go to that highlight self.moveToGoal(nearest_highlight) # Add to "highlights visited" visited_highlights.append(nearest_highlight) # Keep going until all highlights are visited while len(visited_highlights) <= 6: # Calculate next highlight nearest_highlight = self.get_nearest_highlight() # Go to that highlight self.moveToGoal(nearest_highlight) # Add to "highlights visited" visited_highlights.append(nearest_highlight) ''' print( "...And that's all! Thank you for participating in the Devon Tour!" ) def get_nearest_entrance(self, west_entrance, east_entrance): west_entrance_distance = self.distance(west_entrance) east_entrance_distance = self.distance(east_entrance) print("distance to west entrance = " + str(west_entrance_distance)) print("distance to east entrance = " + str(east_entrance_distance)) if west_entrance_distance < east_entrance_distance: return west_entrance else: return east_entrance def distance(self, coordinates): distance = math.sqrt((self.localizer.position.x - coordinates.x)**2 + (self.localizer.position.y - coordinates.y)**2) return distance def point_to_point_mode(self): ''' User chooses which highlights to go to''' choice = self.choose() # Depending on which highlight the user chooses, the robot will # move to the goal if (choice == 1): self.goalReached = self.localizer.moveToGoal(Locations.CSOffice) print( "We've reached the CS Office, where the CS department mainly" + " resides") elif (choice == 2): self.goalReached = self.localizer.moveToGoal(Locations.Atrium) print( "Welcome to the Atrium! This area is used for get-togethers," + "studying, welcome parties, and more!") elif (choice == 3): self.goalReached = self.localizer.moveToGoal( Locations.EastEntrance) print("This is the east entrance") elif (choice == 4): self.goalReached = self.localizer.moveToGoal( Locations.WestEntrance) print("This is the west entrance") elif (choice == 5): self.goalReached = self.localizer.moveToGoal(Locations.ComputerLab) print( "Inside this room here is the CS Computer Lab, where any CS" + " student can come in and use the machines or for TA's office" + " hours") elif (choice == 6): self.goalReached = self.moveToGoal(Locations.ElectronicsLab) print("In this room is the Electronics Lab.") # if choice isn't q and the robot reached its goal if (choice != 'q'): if (self.goalReached): rospy.loginfo("Reached highlight!") # If it fails to reach the goal else: rospy.loginfo("Couldn't reach the highlight, try again") # Loop to keep going until user quits the tour while choice != 'q': choice = self.choose() if (choice == 1): self.goalReached = self.localizer.moveToGoal( Locations.CSOffice) print( "We've reached the CS Office, where the CS department mainly" + " resides") elif (choice == 2): self.goalReached = self.localizer.moveToGoal(Locations.Atrium) print( "Welcome to the Atrium! This area is used for get-togethers," + "studying, welcome parties, and more!") elif (choice == 3): self.goalReached = self.localizer.moveToGoal( Locations.EastEntrance) print("This is the east entrance") elif (choice == 4): self.goalReached = self.localizer.moveToGoal( Locations.WestEntrance) print("This is the west entrance") elif (choice == 5): self.goalReached = self.localizer.moveToGoal( Locations.ComputerLab) print( "Inside this room here is the CS Computer Lab, where any CS" + " student can come in and use the machines or for TA's office" + " hours") elif (choice == 6): self.goalReached = self.localizer.moveToGoal( Locations.ElectronicsLab) print("In this room is the Electronics Lab.") def choose(self): ''' User chooses where they would like to start the tour''' tour = 'q' rospy.loginfo("Initializing P2P Guide mode.") rospy.loginfo("Press a key to go to the highlights:") rospy.loginfo("'1': CS/ECE Office") rospy.loginfo("'2': Atrium") rospy.loginfo("'3': East Entrance") rospy.loginfo("'4': West Entrance") rospy.loginfo("'5': Computer Lab") rospy.loginfo("'6': Electronics Lab") rospy.loginfo("'q': Quit") tour = input() return tour def shutdown(self): '''Shutdown function that's called when user inputs Ctrl + C ''' rospy.loginfo("Stopping Turtlebot") self.motion.halt() rospy.sleep(1) # ensures Turtlebot received the command before def avoid_obstacle(self): '''Checks where a nearby obstacle is and rotates accordingly ''' left_laser = self.sensor.get_left_laser_value() mid_laser = self.sensor.get_mid_laser_value() right_laser = self.sensor.get_right_laser_value() # Check if obstacle in front is 1 ft away if mid_laser <= Distance.FOOT: self.motion.rotate_180() # Check if obstacle is 2 ft to the left elif left_laser < 2 * Distance.FOOT: self.motion.rotate_right() # Check if obstacle is 2 ft to the right elif right_laser < 2 * Distance.FOOT: self.motion.rotate_left()