示例#1
0
    def __init__(self):
        self.dm = DragonbotManager()
        self.dm.pose_off()
        self.exp_sub = rospy.Subscriber("/dragon_teleop_GUI/expressions", String, self.exp_callback)
        self.mot_sub = rospy.Subscriber("/dragon_teleop_GUI/motions", String, self.mot_callback)
        self.ph_sub = rospy.Subscriber("/dragon_teleop_GUI/phrases", String, self.ph_callback)

        self.pose_on_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_on", String, self.pos_on_cb)
        self.x_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_x", Int32, self.x_pos_cb)
        self.y_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_y", Int32, self.y_pos_cb)
        self.z_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_z", Int32, self.z_pos_cb)

        self.look_on_sub = rospy.Subscriber("/dragon_teleop_GUI/lookat_on", String, self.look_on_cb)
        self.xl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_x", Int32, self.x_look_cb)
        self.yl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_y", Int32, self.y_look_cb)
        self.zl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_z", Int32, self.z_look_cb)
        self.blink_sub = rospy.Subscriber("/dragon_teleop_GUI/blink", String, self.blink_cb)

        rospy.loginfo("Ready!")
        self.current_pose = [0,0,0]
        self.pose_on = False

        self.x_min = -2.3
        self.x_max = 2.5
        self.x_range = self.x_max - self.x_min
        self.y_min = -2.49
        self.y_max = 3.4
        self.y_range = self.y_max - self.y_min
        self.z_min = -2.0
        self.z_max = 2.6
        self.z_range = self.z_max - self.z_min

        self.v = 1
        self.a = .05

        self.current_look = [0,0,20]
        self.look_on = False

        self.x_lmin = -300
        self.x_lmax = 300
        self.x_lrange = self.x_lmax - self.x_lmin
        self.y_lmin = -300
        self.y_lmax = 300
        self.y_lrange = self.y_lmax - self.y_lmin
        self.z_lmin = 20
        self.z_lmax = 400
        self.z_lrange = self.z_lmax - self.z_lmin
示例#2
0
def main():
    rospy.init_node('intro_controller')
    dm = DragonbotManager()
    tm = TabletManager()

    rospy.loginfo("loading dialogue")
    with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/intro_dialogue.yaml", 'r') as f:
            s = f.read()

    dialogue_name = "intro"
    dialogue = yaml.load(s)
    
    dg = DialogueManager(tm, dialogue_name, dialogue[dialogue_name])
    
    dm.eye_close()
    tm.change("sleep")
    while not rospy.is_shutdown() and not tm.last_press("/dragon_GUI/sleep") == 0:
        dm.say("intro-05-snore_sleep", wait = False)
        rospy.sleep(6.0)
    dm.express("wakeup")
    dm.eye_open()
    dg.play_dialogue("intro_dialogue")
    dm.eye_close()
    while not rospy.is_shutdown():
        dm.say("intro-05-snore_sleep", wait = False)
        rospy.sleep(6.0)
示例#3
0
def main():
  dm = DragonbotManager()
  tm = TabletManager()
  with open("parenting_center_dialogue.yaml") as f:
    s = f.read()
  dialogues = yaml.load(s)
  
  dg = DialogueManager(dm, tm, "parenting_center_dialogue", dialogues["parenting_center_dialogue"])

  dm.eye_close()
  tm.change("sleep")
  while not rospy.is_shutdown() and not tm.last_press("/dragon_GUI/sleep") == 1:
    dm.say("intro-05-snore_sleep", wait = False)
    rospy.sleep(6.0)
  dm.express("surprised")
  dm.eye_open()

  try:
    dg.play_dialogue("introduction_parenting_center")
  except PanicException:
    dm.eye_close()
    tm.change("sleep")
    return 'AHHH PANIC! SOMETHING BAD HAPPENED!'
  except NextStateException:
    return 'I want to end the dialogue, but nothing bad happened'
  except NextPhraseException:
    # Ignore this
    pass
  # When we get to the end of the dialogue, Chilly goes back to sleep
  dm.eye_close()
  tm.change("sleep")
  return "Finished dialogue successfully"
示例#4
0
class DragonTeleop():
    def __init__(self):
        self.dm = DragonbotManager()
        self.dm.pose_off()
        self.exp_sub = rospy.Subscriber("/dragon_teleop_GUI/expressions", String, self.exp_callback)
        self.mot_sub = rospy.Subscriber("/dragon_teleop_GUI/motions", String, self.mot_callback)
        self.ph_sub = rospy.Subscriber("/dragon_teleop_GUI/phrases", String, self.ph_callback)

        self.pose_on_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_on", String, self.pos_on_cb)
        self.x_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_x", Int32, self.x_pos_cb)
        self.y_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_y", Int32, self.y_pos_cb)
        self.z_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_z", Int32, self.z_pos_cb)

        self.look_on_sub = rospy.Subscriber("/dragon_teleop_GUI/lookat_on", String, self.look_on_cb)
        self.xl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_x", Int32, self.x_look_cb)
        self.yl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_y", Int32, self.y_look_cb)
        self.zl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_z", Int32, self.z_look_cb)
        self.blink_sub = rospy.Subscriber("/dragon_teleop_GUI/blink", String, self.blink_cb)

        rospy.loginfo("Ready!")
        self.current_pose = [0,0,0]
        self.pose_on = False

        self.x_min = -2.3
        self.x_max = 2.5
        self.x_range = self.x_max - self.x_min
        self.y_min = -2.49
        self.y_max = 3.4
        self.y_range = self.y_max - self.y_min
        self.z_min = -2.0
        self.z_max = 2.6
        self.z_range = self.z_max - self.z_min

        self.v = 1
        self.a = .05

        self.current_look = [0,0,20]
        self.look_on = False

        self.x_lmin = -300
        self.x_lmax = 300
        self.x_lrange = self.x_lmax - self.x_lmin
        self.y_lmin = -300
        self.y_lmax = 300
        self.y_lrange = self.y_lmax - self.y_lmin
        self.z_lmin = 20
        self.z_lmax = 400
        self.z_lrange = self.z_lmax - self.z_lmin

    def exp_callback(self, data):
        rospy.loginfo("Got expression: " + data.data)
        self.dm.express(data.data)

    def mot_callback(self, data):
        rospy.loginfo("Got motion: " + data.data)
        self.dm.express(data.data)

    def ph_callback(self, data):
        rospy.loginfo("Got phrase: " + data.data)
        self.dm.say(data.data)

    def blink_cb(self, data):
        rospy.loginfo("Got blink command: " + data.data)
        if data.data == "once":
            self.dm.blink()
        elif data.data == "hold":
            self.dm.eye_close()
        elif data.data == "hold_off":
            self.dm.eye_open()

    def pos_on_cb(self, data):
        rospy.loginfo("Setting pose to: " + data.data)
        if data.data == "on":
            self.current_pose = [0,0,0]
            self.dm.pose(0,0,0)
            self.pose_on = True
        elif data.data == " off":
            self.pose_on = False
            self.dm.pose_off()

    def x_pos_cb(self, data):
        rospy.loginfo("Got x pose value: " + str(data.data))
        if self.pose_on:
            self.current_pose[0] = self.x_min + (float(data.data)/100) * self.x_range
            rospy.loginfo("Setting pose to: " + str(self.current_pose))
            self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a)

    def y_pos_cb(self, data):
        rospy.loginfo("Got y pose value: " + str(data.data))
        if self.pose_on:
            self.current_pose[1] = self.y_min + (float(data.data)/100) * self.y_range
            rospy.loginfo("Setting pose to: " + str(self.current_pose))
            self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a)

    def z_pos_cb(self, data):
        rospy.loginfo("Got z pose value: " + str(data.data))
        if self.pose_on:
            self.current_pose[2] = self.z_min + (float(data.data)/100) * self.z_range
            rospy.loginfo("Setting pose to: " + str(self.current_pose))
            self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a)

    def look_on_cb(self, data):
        rospy.loginfo("Setting lookat to: " + data.data)
        if data.data == "on":
            self.current_look = [0,0,20]
            self.dm.lookat(0,0,20)
            self.look_on = True
        elif data.data == " off":
            self.look_on = False
            self.dm.lookat_off()

    def x_look_cb(self, data):
        rospy.loginfo("Got x lookat value: " + str(data.data))
        if self.look_on:
            self.current_look[0] = self.x_lmin + (float(data.data)/100) * self.x_lrange
            rospy.loginfo("Setting lookat to: " + str(self.current_look))
            self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2])

    def y_look_cb(self, data):
        rospy.loginfo("Got y lookat value: " + str(data.data))
        if self.look_on:
            self.current_look[1] = self.y_lmin + (float(data.data)/100) * self.y_lrange
            rospy.loginfo("Setting lookat to: " + str(self.current_look))
            self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2])

    def z_look_cb(self, data):
        rospy.loginfo("Got z lookat value: " + str(data.data))
        if self.look_on:
            self.current_look[2] = self.z_lmin + (float(data.data)/100) * self.z_lrange
            rospy.loginfo("Setting lookat to: " + str(self.current_look))
            self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2])
示例#5
0
def main():
    rospy.init_node('experiment_controller')

    if not rospy.has_param("~max_time"):
        rospy.set_param("~max_time", 600)
    rospy.set_param("~start_time", rospy.Time.now().secs)

    day = rospy.get_param("~lesson")
    day_num = rospy.get_param("~day")
    
    sm = smach.StateMachine(outcomes=['end'])

    lesson_list = {'lunchbox':("whole_grains", "drinks"),
                   'snacks':("snacks1", "snacks2"),
                   'breakfast':("cereal", "breakfast"),
                   'dinner':("sides1","sides2")}

   

    dm = DragonbotManager()
    dm.eye_open()
    tm = TabletManager()

    #info: day, lessons
    lessons = lesson_list[day]
    day = day + str(day_num)
    info = (day,lessons)
    
    rospy.loginfo("Reading food phrases file.")
    if day_num == 1:
        with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/day1_food_phrases.yaml", 'r') as f:
            s = f.read()
    elif day_num == 2:
        with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/day2_food_phrases.yaml", 'r') as f:
            s = f.read()
    else:
        print "Usage: day number must be 1 or 2"
        sys.exit()

    # file format is:
    # lesson:
    #  intro: phrase_id
    #  reminder: phrase_idfg
    #  no_choice: phrase_id
    #  foodname: (food chosen before)
    #    foodname: phrase_id (food chosen after)
    # index as food_info["lesson"]["choice1"]["choice2"]
    # nb: food_info["lesson"]["none"].keys() will give all foods (and "none")
    food_info = yaml.load(s)

    rospy.loginfo("Reading other dialogue phrases file.")
    with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/dialogue_specification.yaml", 'r') as f:
        s = f.read()
    
    # file format is:
    
    dialogue_info = yaml.load(s)
    

    #rospy.loginfo("Loading phrase information file.")
    #dm.load_phrases("phrases.yaml")
    #rospy.loginfo("Done loading files.")

    if day_num == 1:
        food_state = FoodChoiceDay1(dm, tm, info, dialogue_info[day + "_foods"],food_info)
    elif day_num == 2:
        food_state = FoodChoiceDay2(dm, tm, info, dialogue_info[day + "_foods"],food_info)
 
    with sm:
        smach.StateMachine.add('WORKOUT', Workout(dm, tm, info, dialogue_info[day + "_workout"]),
                               transitions={'panic':'SLEEP',
                                            'continue':'WORKOUT',
                                            'end':'OUTRO',
                                            'timeout':'OUTRO'})


        smach.StateMachine.add('SLEEP', Sleep(dm, tm, info),
                               transitions={'wakeup':'INTRO',
                                            'done':'end'})
        smach.StateMachine.add('INTRO', Intro(dm, tm, info, dialogue_info[day + "_intro"]),
                               transitions={'panic':'SLEEP',
                                            'end':'F_CHOICE'})
        smach.StateMachine.add('F_CHOICE', food_state,
                               transitions={'panic':'SLEEP',
                                            'next_round':'F_CHOICE',
                                            'end':'WORKOUT',
                                            'timeout':'OUTRO'})


 
        smach.StateMachine.add('OUTRO', Outro(dm, tm, info, dialogue_info[day + "_outro"]),
                               transitions={'end':'SLEEP',
                                            'panic':'SLEEP'})

    outcome = sm.execute()