3. Implementation Of Coffeebot

Monitor the battery status of the kobuki and netbook:

$python kobuki_battery.py       $python netbook_battery.py  /  $rostopic echo /laptop_charge

Autonomous Driving 

TURTLEBOT:

roslaunch turtlebot_bringup minimal.launch

roslaunch turtlebot_navigation amcl_demo.launch map_file:=/tmp/M201_map.yaml  (right result : odom received )

Troubleshooting:save the map in another directory, and when you need to load the map, copy the map in the /tmp directory. And then run the command.

WORKSTATION:

roslaunch turtlebot_rviz_launchers view_navigation.launch --screen   

# rviz should open showing your map. 

estimate the 2D pose of the turtlebot and select "2D Nav Goal" , then the turtlebot could drive automatically to your desired location.

Going Forward and Avoiding Obstacles

bring up the turtlebot, load and show the map, and then run the script: goforward_and_avoid_obstacle.py 

Going to a specific location on your map 

load and show the map, update the script with a pose where you want the turtlebot to go.

select "Publish Point", on the bottom left corner of the screen you can see the coordinates of the points, noticing that the third value is altitude. After updating the value of the goal.target_pose.pose, run the script.

(what is "rospy.duration time limit"?  )

Setting optimal angles for customers

url : http://learn.turtlebot.com/2015/02/01/26/

Configure coffeebot's web server

url : http://learn.turtlebot.com/2015/02/01/20/

Configure coffee_bot.py

(change the public dns, x,y, max_charge)

#Coffee Bot

import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
import json
import urllib2
import time #for sleep()
import roslib
from kobuki_msgs.msg import PowerSystemEvent, AutoDockingAction, AutoDockingGoal, SensorState #for kobuki base power and auto docking
from kobuki_msgs.msg import ButtonEvent #for kobuki base's b0 button
from smart_battery_msgs.msg import SmartBatteryStatus #for netbook battery
import math #for comparing if Kobuki's power has changed using fabs

class turtlebot_coffee():
    ######## CHANGE THE FOLLOWING VALUES #########
    server_public_dns = 'http://ec2-52-35-215-167.us-west-2.compute.amazonaws.com' #must start with http:// .  Don't include a trailing "/"
    near_docking_station_x = -1.88 #x coordinate for pose approx 1 meter from docking station
    near_docking_station_y = 0.06 #y coordinate for pose approx 1 meter from docking station    
    ######## END CHANGE THE FOLLOWING VALUES #########

    ####### OPTIONVAL VALUES TO CHANGE ##########
    kobuki_base_max_charge = 160 
    #we're using the extended battery.  Your battery may have a different max charge value.  The maximum charge for kobuki base can be determined by running:
    #rostopic echo /mobile_base/sensors/core 
    #and viewing the "battery" value.  This value is used to determine kobuki's battery status %.
    ####### END OPTIONVAL VALUES TO CHANGE ##########
    
    # defaults
    move_base = False # _init_ converts this to a MoveBaseAction that is used to set the goals
    battery_is_low = False # is kobuki's battery low?
    battery_is_low_netbook = False # is the notebook's battery low?
    netbook_previous_battery_level = 100 # what was the previous netbook battery level (used to know if it's changed)
    kobuki_previous_battery_level = 1000 #1000 isn't possible.  Just a large fake # so the script starts believing the battery is fine
    charging_at_dock_station = False #can't leave docking station until it's full because battery was low
    proactive_charging_at_dock_station = False #can leave docking station as soon as a coffee request comes in because battery is fine
    count_no_one_needs_coffee_in_a_row = 0 #keeps track of how many times in a row we receive "no one needs coffee".  If there is considerable down time TurtleBot will return to the docking station to proactively charge itself.
    how_many_no_one_needs_coffee_before_proactive_charging = 5 # how many times should we receive "no one needs coffee" prior to proactively returning to the docking station
    cannot_move_until_b0_is_pressed = False # should TurtleBot stay still until B0 is pressed (e.g. while the person is brewing coffee)?

    def __init__(self):
	#initialize ros node
        rospy.init_node('turtlebot_coffee', anonymous=False)

	#what to do if shut down (e.g. ctrl + C or failure)
	rospy.on_shutdown(self.shutdown)
	
	#tell the action client that we want to spin a thread by default
	self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
	rospy.loginfo("wait for the action server to come up")
	#allow up to 30 seconds for the action server to come up
	self.move_base.wait_for_server(rospy.Duration(30))

	#monitor Kobuki's power and charging status.  If an event occurs (low battery, charging, not charging etc) call function SensorPowerEventCallback
	rospy.Subscriber("/mobile_base/sensors/core",SensorState,self.SensorPowerEventCallback)

	#monitor netbook's battery power
	rospy.Subscriber("/laptop_charge/",SmartBatteryStatus,self.NetbookPowerEventCallback)

	#to avoid TurtleBot from driving to another pose while someone is making coffee ... TurtleBot isn't allowed to move until the person presses the B0 button.  To implement this we need to monitor the kobuki button events
	rospy.Subscriber("/mobile_base/events/button",ButtonEvent,self.ButtonEventCallback)

    def deliver_coffee(self):
	#if someone is currently making coffee don't move!
	if(self.cannot_move_until_b0_is_pressed):
		rospy.loginfo("Waiting for button B0 to be pressed.")
		time.sleep(2)
		return True

	#before we deliver the next coffee... how is power looking? If low go recharge first at the docking station.
	if(self.INeedPower()):
		return True

	#Power is fine so let's see if anyone needs coffee...
	rospy.loginfo("Anyone need coffee?")
	#we'll send a goal to the robot to tell it to move to a pose that's near the docking station
	goal = MoveBaseGoal()
	goal.target_pose.header.frame_id = 'map'
	goal.target_pose.header.stamp = rospy.Time.now()
	
	#call the server and "pop" the next pending customer's pose (if one is pending) from the stack
	data = json.load(urllib2.urlopen(self.server_public_dns + "/turtlebot-server/coffee_queue.php?pop"))
	if(data["status"] == "pending"): #someone is pending coffee!  Oh ya... let's get moving
		#If we're at the charging station back up 0.2 meters to avoid collision with dock
		self.DoWeNeedToBackUpFromChargingStation()

		#Where are they?  Set the person's pose
		goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"])))

		#start moving
        	self.move_base.send_goal(goal)

		#allow TurtleBot up to 60 seconds to complete task
		success = self.move_base.wait_for_result(rospy.Duration(60)) 

	
		if not success:
			#failed to reach goal (e.g. TurtleBot can't find a way to go to the location)
        	        self.move_base.cancel_goal()
        	        rospy.loginfo("The base failed to reach the desired pose")
			#tell the server that this pose failed (so it won't try it again)
			data = json.load(urllib2.urlopen(self.server_public_dns + "/turtlebot-server/coffee_queue.php?update&id=" + data["id"] + "&status=failed"))
    		else:
			# We made it!
			state = self.move_base.get_state()
			if state == GoalStatus.SUCCEEDED:
			    rospy.loginfo("Hooray, reached the desired pose!  Press B0 to allow TurtleBot to continue.")
			    #tell TurtleBot not to move until the customer presses B0
			    self.cannot_move_until_b0_is_pressed = True
			    self.count_no_one_needs_coffee_in_a_row = 0 #reset to 0
			    #tell the server that the pose was completed
			    data = json.load(urllib2.urlopen(self.server_public_dns + "/turtlebot-server/coffee_queue.php?update&id=" + data["id"] + "&status=complete"))

	else: #no one needs coffee :(		
		self.count_no_one_needs_coffee_in_a_row = self.count_no_one_needs_coffee_in_a_row + 1 #increment so we know how many times in a row no one needed coffee
		rospy.loginfo("No one needs coffee #" + str(self.count_no_one_needs_coffee_in_a_row))
		#considering there is nothing to do... should we charge?
		if(self.count_no_one_needs_coffee_in_a_row > self.how_many_no_one_needs_coffee_before_proactive_charging and not self.charging_at_dock_station):
			rospy.loginfo("Battery is fine but considering no one wants coffee ... Going to docking station.")
			self.DockWithChargingStation() #tell TurtleBot to dock with the charging station
			self.proactive_charging_at_dock_station = True
		else:
			time.sleep(2) #wait 2 seconds before asking the server if there are pending coffee needs

	return True

    def ButtonEventCallback(self,data):
	#From https://github.com/yujinrobot/kobuki/blob/f99e495b2b3be1e62495119809c58ccb58909f67/kobuki_testsuite/scripts/test_events.py
	if ( data.button == ButtonEvent.Button0 ) :
		self.cannot_move_until_b0_is_pressed = False

    def DoWeNeedToBackUpFromChargingStation(self):
	#if you set a goal while it's docked it tends to run into the docking station while turning.  Tell it to back up a little before initiliazing goals.
	if(self.proactive_charging_at_dock_station or self.charging_at_dock_station):
		rospy.loginfo("We're at the docking station.  Back up before next goal.")
		self.proactive_charging_at_dock_station = False
		cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
		# Twist is a datatype for velocity
		move_cmd = Twist()
		# let's go forward at 0.1 m/s
		move_cmd.linear.x = -0.1
		# let's turn at 0 radians/s
		move_cmd.angular.z = 0

		r = rospy.Rate(10);
		# as long as you haven't ctrl + c keeping doing...
		temp_count = 0
		#go back at 0.1 m/s for 2 seconds
		while (not rospy.is_shutdown() and temp_count < 20):
			# publish the velocity
			cmd_vel.publish(move_cmd)
			# wait for 0.1 seconds (10 HZ) and publish again
			temp_count = temp_count + 1
			r.sleep()
		#make sure TurtleBot stops by sending a default Twist()
		cmd_vel.publish(Twist())
		return True

    def INeedPower(self):
	#are we currently charging at the docking station?  If yes only continue if we're not fully charged
	if(self.charging_at_dock_station and (self.battery_is_low or self.battery_is_low_netbook)):
		rospy.loginfo("I'm charging and will continue when I'm sufficiently charged")
		time.sleep(30)
		return True
	#are we not currently charging and is either battery low?  If yes, go to docking station.
	if(not self.charging_at_dock_station and (self.battery_is_low or self.battery_is_low_netbook)):
		rospy.loginfo("Battery is low. Going to docking station.")
		self.DockWithChargingStation() #tell TurtleBot to dock with the charging station
		return True
	return False

    def SensorPowerEventCallback(self,data):
	#kobuki's batttery value tends to bounce up and down 1 constantly so only report if difference greater than 1
	if(math.fabs(int(data.battery) - self.kobuki_previous_battery_level) > 2):
		rospy.loginfo("Kobuki's battery is now: " + str(round(float(data.battery) / float(self.kobuki_base_max_charge) * 100)) + "%")
		self.kobuki_previous_battery_level = int(data.battery)

	if(int(data.charger) == 0) :
		if(self.charging_at_dock_station):
			rospy.loginfo("Stopped charging at docking station")
		self.charging_at_dock_station = False
	else:
		if(not self.charging_at_dock_station):
			rospy.loginfo("Charging at docking station")
		self.charging_at_dock_station = True

	
	if ( round(float(data.battery) / float(self.kobuki_base_max_charge) * 100) < 50) :
		if(not self.battery_is_low):
			rospy.loginfo("Kobuki battery is low")
		self.battery_is_low = True
	elif ( round(float(data.battery) / float(self.kobuki_base_max_charge) * 100) > 60): #the logic of not using the same value (e.g. 50) for both the battery is low & battery is fine is that it'll leave and immediatly return for more power.  The reason why we don't use == 100 is that we hope that proactive charging between coffee deliveries will charge it soon and we don't want people waiting.
		if(self.battery_is_low):
			rospy.loginfo("Kobuki battery is fine")
		self.battery_is_low = False

		
    def NetbookPowerEventCallback(self,data):
	#has the netbook's power level changed?
	if(int(data.percentage) != self.netbook_previous_battery_level):
		rospy.loginfo("Notebook's battery is now: " + str(data.percentage) + "%")
		self.netbook_previous_battery_level = int(data.percentage)
	#is the netbook's power low?
	if(int(data.percentage) < 50): #50 is the percent of total power
		self.battery_is_low_netbook = True
	elif(int(data.percentage) > 60): #the logic of not using the same value (e.g. 50) for both the battery is low & battery is fine is that it'll leave and immediatly return for more power.  The reason why we don't use == 100 is that we hope that proactive charging between coffee deliveries will charge it soon and we don't want people waiting.
		self.battery_is_low_netbook = False

    def DockWithChargingStation(self):
	#before we can run auto-docking we need to be close to the docking station..
	if(not self.GoCloseToTheChargingStation()):
		return False
	
	#We're close to the docking station... so let's dock
	return self.WereCloseDock()

    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")
		self.charging_at_dock_station = True #The callback which detects the docking status can take up to 3 seconds to update which was causing coffee bot to try and redock (presuming it failed) even when the dock was successful.  Therefore hardcoding this value after success.
		return True
	else:
		rospy.loginfo("Auto_docking failed")
		return False
		

    def GoCloseToTheChargingStation(self):
	#the auto docking script works well as long as you are roughly 1 meter from the docking station.  So let's get close first...
	rospy.loginfo("Let's go near the docking station")

	goal = MoveBaseGoal()
	goal.target_pose.header.frame_id = 'map'
	goal.target_pose.header.stamp = rospy.Time.now()

	#set a Pose near the docking station
	goal.target_pose.pose = Pose(Point(float(self.near_docking_station_x), float(self.near_docking_station_y), float(0)), Quaternion(float(0), float(0), float(0.892), float(-1.5)))

	#start moving
	self.move_base.send_goal(goal)

	#allow TurtleBot up to 60 seconds to get close to 
	success = self.move_base.wait_for_result(rospy.Duration(60)) 

	if not success:
	        self.move_base.cancel_goal()
	        rospy.loginfo("The base failed to reach the desired pose near the charging station")
		return False
	else:
		# We made it!
		state = self.move_base.get_state()
		if state == GoalStatus.SUCCEEDED:
		    rospy.loginfo("Hooray, reached the desired pose near the charging station")
		    return True



    def shutdown(self):
        rospy.loginfo("Stop")


if __name__ == '__main__':
    delivery_checks = 0 #just for troubleshooting to see how many times we called the server to check for pending coffee
    try:
        coffebot = turtlebot_coffee()
        #keep checking for deliver_coffee until we shutdown the script with ctrl + c
	while(coffebot.deliver_coffee() and not rospy.is_shutdown()):
                delivery_checks = delivery_checks + 1

    except rospy.ROSInterruptException:
        rospy.loginfo("Exception thrown")

Some details:  Why does it go in circles?(localization)

Turtlebot is looking around for features, comparing them to the map and using that to determine where it is. This process is also called localization.

The turtlebot uses odometry and laser scan data to localize itself. 

The openni_manager uses the kinect pointcloud data to create laser scan data for use for amcl.

Amcl (adaptive Monte Carlo localization) works by figuring out where the robot would need to be on the map in order its laser scans to make sense. Each possible location is represented by a "particle" and particles with laser scans that do not match well are removed resulting in a group of particles representing the location of the robot in the map. 

The amcl node uses the particle positions to compute and publish the transform from map to base_link.