def WereCloseDock(self): # The following will start the AutoDockingAction which will automatically find and dock TurtleBot # with the docking station as long as it's near the docking station when started self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) rospy.loginfo("waiting for auto_docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo( "Sending auto_docking goal and waiting for result (times out in 180 seconds and will try again if required)" ) self._client.send_goal(goal) # Give the auto docking script 180 seconds. It can take a while if it retries. success = self._client.wait_for_result(rospy.Duration(180)) if success: rospy.loginfo("Auto_docking succeeded") # The callback which detects the docking status can take up to 3 seconds to update which was causing bot # to try and redock (presuming it failed) even when the dock was successful. # Therefore hardcoding this value after success. self.charging_at_dock_station = True return True else: rospy.loginfo("Auto_docking failed") return False
def DockWithDockingStation(self): # Automatically docks robot with docking station if the robot is close to the docking station self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) rospy.loginfo("Waiting for Auto Docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo("Sending auto docking goal and waiting for results") success = self._client.wait_for_result( rospy.Duration(self.wait_autodock_server_to_start)) if success: rospy.loginfo("Auto_docking succeeded") self.at_docking_station = True else: rospy.loginfo("Auto_docking failed") self.at_docking_station = False return self.escort_user()
def auto_dock(self): rospy.loginfo("Attempting to autonomously dock to charging station.\n") try: # set docking action client ad_client = actionlib.SimpleActionClient( "/" + self.robot_name + "/dock_drive_action", AutoDockingAction) # connect to Action Server rospy.loginfo("Wating for auto_dock Action Server...") while not ad_client.wait_for_server(timeout=rospy.Duration( secs=5)): if rospy.is_shutdown(): return False rospy.loginfo("Found auto_dock Action Server") # set docking goal ad_goal = AutoDockingGoal() ad_client.send_goal(ad_goal) rospy.on_shutdown(ad_client.cancel_goal) rospy.loginfo("Attemping to dock...") # run action and wait 120 seconds for result ad_client.wait_for_result(rospy.Duration(secs=120)) if ad_client.get_result(): rospy.loginfo("Docking Successful.") else: rospy.loginfo("Docking Unsuccessful.") # catch interrupts except rospy.ROSInterruptException(): rospy.logerr("Docking interrupted by user.") ad_client.cancel_goal() except Exception: rospy.logerr("Docking interrupted unexpectedly.") ad_client.cancel_goal()
def dock_with_base(self): self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) rospy.loginfo("waiting for auto_docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo( "Sending auto_docking goal and waiting for result (times out in 180 seconds and will try again if required)")
def execute(self, userdata): # add timeout setting client = actionlib.SimpleActionClient('dock_drive_action', AutoDockingAction) client.wait_for_server() goal = AutoDockingGoal() client.send_goal(goal) rospy.on_shutdown(client.cancel_goal) client.wait_for_result() return 'success'
def dock(self): """ dock to nearby power loading station :return: True if succeeded """ if not self.dockingClient.gh or not self.dockingClient.get_state() in (GoalStatus.SUCCEEDED, GoalStatus.PENDING, GoalStatus.ACTIVE): self.dockingClient.send_goal(AutoDockingGoal()) #TODO test if parameter is required rospy.loginfo(self.name + ": docking") if self.dockingClient.get_state() == GoalStatus.SUCCEEDED: self.dockingClient.stop_tracking_goal() rospy.loginfo(self.name + ": docking succeeded") self.docked = True return True return False
def dock_drive_client(): # add timeout setting client = actionlib.SimpleActionClient('dock_drive_action', AutoDockingAction) while not client.wait_for_server(rospy.Duration(5.0)): if rospy.is_shutdown(): return print 'Action server is not connected yet. still waiting...' goal = AutoDockingGoal(); client.send_goal(goal, doneCb, activeCb, feedbackCb) print 'Goal: Sent.' rospy.on_shutdown(client.cancel_goal) client.wait_for_result() #print ' - status:', client.get_goal_status_text() return client.get_result()
def docking_cb(data): global current_status print 'docking: current status is ' + str(current_status) if current_status == 0: client_mb = actionlib.SimpleActionClient('move_base', MoveBaseAction) while not client_mb.wait_for_server(rospy.Duration(5)): if rospy.is_shutdown(): return print 'Action server is not connected yet. still waiting...' goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = pose_x goal.target_pose.pose.position.y = pose_y goal.target_pose.pose.orientation.w = 1.0 print 'goal ready for move_base' client_mb.send_goal(goal) client_mb.wait_for_result() print 'result received' client_dk = actionlib.SimpleActionClient('dock_drive_action', AutoDockingAction) while not client_dk.wait_for_server(rospy.Duration(5)): if rospy.is_shutdown(): return print 'Action server is not connected yet. still waiting...' goal = AutoDockingGoal() client_dk.send_goal(goal, done_cb, active_cb, feedback_cb) print 'Goal: Sent.' rospy.on_shutdown(client_dk.cancel_goal) client_dk.wait_for_result() else: pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=1) msg = Twist() msg.linear.x = -0.1 r = rospy.Rate(10) k = 0 while k < 50: pub.publish(msg) r.sleep() k += 1
def __init__(self): self.IR_sensor = False self.start = False self.sub = { 'dock_ir': rospy.Subscriber('mobile_base/sensors/dock_ir', DockInfraRed, self.dock_ir_call_back, queue_size=1) } self.dock_ir = DockInfraRed() self.goal = AutoDockingGoal() self.client = actionlib.SimpleActionClient('dock_drive_action', AutoDockingAction) self.state = '' self.srv_client = rospy.ServiceProxy('start_docking_srv', Empty) self.dock_srv = rospy.Service('allow_dockin_srv', SetBool, self.cb_allow_docking)
def execute(self, userdata): goal = AutoDockingGoal() rospy.loginfo( "Sending auto_docking goal and waiting for result (times out in 180 seconds and will try again if required)" ) self._client.send_goal(goal) success = self._client.wait_for_result(rospy.Duration(60)) if success: rospy.loginfo("Auto_docking successful") # The callback which detects the docking status can take up to 3 seconds to update which was # causing the robbot to try and redock (presuming it failed) even when the dock was successful. # Therefore hardcoding this value after success. self.charging_at_dock_station = True success = True return 'docking_complete' else: rospy.loginfo("Auto_docking failed") success = False return 'docking_failed'
def start_docking(self): self._client = actionlib.SimpleActionClient("/dock_drive_action", AutoDockingAction) rospy.loginfo("Waiting for auto_docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo( "Sending auto_docking goal and waiting for result (times out in 180 seconds and will try again if required)" ) self._client.send_goal(goal) success = self._client.wait_for_result(rospy.Duration(180)) if success: rospy.loginfo("auto_docking succeeded") self.is_charging = True return True else: rospy.loginfo("auto_docking failed") return False
def DockWithDockingStation(self): # Automatically docks robot with docking station if the robot is close to the docking station self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) rospy.loginfo("Waiting for Auto Docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo("Sending auto docking goal and waiting for results") self._client.send_goal(goal) success = self._client.wait_for_result( rospy.Duration(self.wait_autodock_server_to_start)) if success: rospy.loginfo("Auto_docking succeeded") self.at_docking_station = True # Information to inform Arduino to turn head into proper position, publishing to 'head_state' topic self.head_position = 0 rospy.loginfo(self.head_position) self.head.publish(self.head_position) self.EscortUser() return True else: rospy.loginfo("Auto_docking failed") self.error_flag = True self.error_location = 'DockWithDockingStation failed' self.led_state = 6 self.led.publish(self.led_state) self.at_docking_station = False self.EscortUser() return False
def dock(self): goal = AutoDockingGoal() self.dock_client.send_goal(goal) self.dock_client.wait_for_result() print(self.dock_client.get_result())
def _main_loop(self): while not rospy.is_shutdown(): if self._state == 0: # wait for initial pose to be set. may need to switch this to waiting for a key press if self._initial_pose_set: print("Initial pose set. Moving backwards out of base...") self._state = 1 self._state_start_time = rospy.get_time() elif self._state == 1: # move the robot backward at constant velocity for some time if rospy.get_time() - self._state_start_time < 5.0: vel = -0.2 cmd_vel = Twist() cmd_vel.linear.x = vel self._cmd_vel_pub.publish(cmd_vel) else: print( "Robot (probably) out of base. Starting spinning to improve localization convergence." ) self._state = 2 cur_pose_as_goal = self.get_pose_as_goal() self._final_waypoint.target_pose.pose.orientation = cur_pose_as_goal.target_pose.pose.orientation self._state_start_time = rospy.get_time() elif self._state == 2: # spin robot to localize, then set point as final waypoint if rospy.get_time() - self._state_start_time < 6.28: vel = 2 cmd_vel = Twist() cmd_vel.angular.z = vel self._cmd_vel_pub.publish(cmd_vel) else: print( "Spinning complete. Storing current pose as final waypoint, then starting waypoints." ) cur_pose_as_goal = self.get_pose_as_goal() self._final_waypoint.target_pose.pose.position = cur_pose_as_goal.target_pose.pose.position print("test ", self._final_waypoint.target_pose.pose.orientation) # setup the waypoints action client while not self._wp_ac.wait_for_server(rospy.Duration(5)): print("Waiting for waypoints action server.") goal = GoToWaypointsGoal() goal.waypoints_file = "waypoints.txt" # todo: non hardcode self._wp_ac.send_goal(goal, feedback_cb=self._wp_feedback_cb) print("Waypoints goal sent.") self._state = 3 self._state_start_time = rospy.get_time() elif self._state == 3: # initialize the waypoints node, go to all of the waypoints. wait for that node to finish if self._wp_ac.get_state() == GoalStatus.SUCCEEDED: print("All waypoints complete! Moving to dock waypoint.") self._state = 4 self._state_start_time = rospy.get_time() elif self._state == 4: # possible state for checking preliminary sentence quality and doing a 2nd lap. may not use. while not self._ac.wait_for_server(rospy.Duration(5)): print("Waiting for move_base action server.") self._ac.send_goal(self._final_waypoint) print("Final waypoint sent as goal.") self._state = 5 elif self._state == 5: # go to the final waypoint, prepare to enter dock mb_ac_state = self._ac.get_state() if mb_ac_state == GoalStatus.SUCCEEDED: print("Final waypoint reached. Attempting to enter dock.") # setup the auto docking action client while not self._auto_dock_ac.wait_for_server( rospy.Duration(5)): print("Waiting for dock action server.") goal = AutoDockingGoal() self._auto_dock_ac.send_goal( goal, feedback_cb=self._dock_feedback_cb) print("Docking goal sent.") self._state = 6 self._state_start_time = rospy.get_time() elif mb_ac_state == GoalStatus.ABORTED or mb_ac_state == GoalStatus.REJECTED: rospy.logerr( "waypoint near dock not reached (check state 5 of state machine" ) self._state = 8 elif self._state == 6: # attempt to enter dock. if timeout OR sucessfully docked, go to next state if self._auto_dock_ac.get_state( ) == GoalStatus.SUCCEEDED or self._auto_dock_fb == "DOCKED_IN": print("Docked! Starting sentence analysis/speaking.") # todo: add in command to stop the docking action client here self._state = 7 self._state_start_time = rospy.get_time() elif rospy.get_time() - self._state_start_time > 25.0: print( "Docking timed out. Starting sentence analysis/speaking." ) # todo: add in command to actually stop docking in this case via action client self._state = 7 self._state_start_time = rospy.get_time() elif self._state == 7: # sentence analysis/speaking words = rospy.get_param("/stored_keywords", []) if rospy.get_time() - self._state_start_time > 5.0: print("Sentence: ", words) self._state_start_time = rospy.get_time() self.rate.sleep()