def __init__(self):
rospy.init_node('demo2_node', anonymous=False)
rospy.on_shutdown(self.cleanup)
# Get the sensor information from the database, based on ID
self.fridge_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:6")
self.tray_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:26")
self.large_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:25")
self.small_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:21")
# Set initial values
self.fridge_opened = False
self.user_sat_down = False
self.drink_placed = False
self.ctrs = CommandToRobotSender()
class Demo2(object):
## init function
def __init__(self):
rospy.init_node('demo2_node', anonymous=False)
rospy.on_shutdown(self.cleanup)
# Get the sensor information from the database, based on ID
self.fridge_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:6")
self.tray_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:26")
self.large_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:25")
self.small_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:21")
# Set initial values
self.fridge_opened = False
self.user_sat_down = False
self.drink_placed = False
self.ctrs = CommandToRobotSender()
## As soon as we stop the script (or there is a other reason for the scipt to stop) it is best to stop the robot
def cleanup(self):
# stop the robot
rospy.loginfo("Stopping now..")
self.ctrs.move("base","stop", False)
## The run function
def run(self):
# Get the current values of the sensors
fridge_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:6")
tray_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:26")
large_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:25")
small_sofa_value = SensorsInDatabase().get_value_by_communication_id("ZWAVE:21")
# Fridge opened?
if(fridge_value != self.fridge_value and self.fridge_opened == False):
rospy.loginfo("Fridge opened, heading there now..")
self.ctrs.move("base","kitchen_fridge", False)
self.fridge_opened = True
self.fridge_value = fridge_value
# Fridge has been opened and something has been placed in the holder?
if(tray_value != self.tray_value and self.user_sat_down == False):
rospy.loginfo("User placed drink")
self.drink_placed = True
self.tray_value = tray_value
# Fridge was opened, tray is full and the user sits. Time to bring the goodies!
if(self.fridge_opened == True and self.drink_placed == True and self.user_sat_down == False):
# Both seats occupied?
if(large_sofa_value != self.large_sofa_value and small_sofa_value != self.small_sofa_value):
self.user_sat_down = True
self.large_sofa_value = large_sofa_value
self.small_sofa_value = small_sofa_value
rospy.loginfo("Both pads occupied, choosing where to go..")
# Choose randomly where to go
rand = randrange(0, 10)
if(rand <= 5):
rospy.loginfo("Going to large sofa..")
self.ctrs.move("base","large_sofa_offer", False)
elif(rand > 5):
rospy.loginfo("Going to small sofa..")
self.ctrs.move("base","small_sofa_offer", False)
# Not both pads occupied, checking if one of them is
else:
rospy.loginfo("Checking if user sits..")
# Large sofa occupied?
if(large_sofa_value != self.large_sofa_value):
rospy.loginfo("Going to large sofa..")
self.user_sat_down = True
self.large_sofa_value = large_sofa_value
self.ctrs.move("base","large_sofa_offer", False)
# Small sofa occupied?
elif(small_sofa_value != self.small_sofa_value):
rospy.loginfo("Going to small sofa..")
self.user_sat_down = True
self.small_sofa_value = small_sofa_value
self.ctrs.move("base","small_sofa_offer", False)
# User sat down, robot at user location, user picked drink out of the holder
# Time to return to the charging station
elif(tray_value != self.tray_value and self.user_sat_down == True):
rospy.loginfo("Delivered drink to the user, now heading to charging station..")
self.ctrs.move("base","charging_station", False)
self.fridge_value = fridge_value
self.user_sat_down = False
self.fridge_opened = False
self.drink_placed = False
self.tray_value = tray_value
