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()
