Skip to content

Commit

Permalink
Merge pull request #863 from tue-robotics/develop_cleanup
Browse files Browse the repository at this point in the history 
Allow for testing on robot
  • Loading branch information
TheToolman committed Jun 20, 2019
2 parents b2042f0 + 0c27162 commit 13ae085
Show file tree
Hide file tree
Showing 5 changed files with 154 additions and 646 deletions.
2 changes: 1 addition & 1 deletion challenge_cleanup/src/challenge_cleanup/clean_inspect.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ def __init__(self, robot, location_des):
transitions={'done': "HANDLE_DETECTED_ENTITIES"})

# Determine the next state, either it is the next iter or done
#next_state = "NAVIGATE_%d" % (i + 1) if i + 1 < len(segment_areas) else "done"
# next_state = "NAVIGATE_%d" % (i + 1) if i + 1 < len(segment_areas) else "done"

smach.StateMachine.add("SAY_UNREACHABLE",
robot_smach_states.Say(robot, ["I failed to inspect the %s" % location_id], block=True),
Expand Down
295 changes: 149 additions & 146 deletions challenge_cleanup/src/challenge_cleanup/cleanup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,8 @@
import hmi

import robot_smach_states
from robot_smach_states.util.designators import VariableDesignator, VariableWriter, EntityByIdDesignator
import robot_smach_states.util.designators as ds
from clean_inspect import CleanInspect
from robot_smach_states.utility import SetInitialPose

from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_cleanup')
Expand All @@ -27,14 +26,18 @@ def __init__(self, robot):
self._robot = robot

def execute(self, userdata):
# Look for trash units
# Look for trash units; can be in living_room and kitchen.
# THIS IS ARENA DEPENDANT!! (Should be handled in a different way?)
# There should be an 'underscore_rule' : trash_bin or trashbin???
# (Different between rgo2019 and robotics_testlab knowledge)

ids = [e.id for e in self._robot.ed.get_entities()]
for loc in challenge_knowledge.cleaning_locations:
if loc["room"] == "living_room":
if "trash_bin" not in ids:
return "failed"
# if loc["room"] == "living_room":
# if "trash_bin" not in ids:
# return "failed"
if loc["room"] == "kitchen":
if "trash_can" not in ids:
if "trashbin" not in ids:
return "failed"

# Make sure the world model and local knowledge match
Expand All @@ -46,187 +49,187 @@ def execute(self, userdata):

return "done"

def collect_cleanup_entities(room):
"""
Create list of points to visit in the selected room
:param room:
:return:
"""
cleaning_locations = []
for loc in challenge_knowledge.cleaning_locations:
if loc["room"] == room:
cleaning_locations.append(loc)
return cleaning_locations
class AskWhichRoomToClean(smach.State):
# Logic in this code is still flawed. No correct repetition.....

def __init__(self, robot, roomw, answerw, cleanup_locationsw):
smach.State.__init__(self, outcomes=["failed", "done"])
self.robot = robot
self.roomw = roomw
self.answerw = answerw
self.cleanup_locationsw = cleanup_locationsw

def collect_cleanup_locations(self):
cleaning_locations = []
for loc in challenge_knowledge.cleaning_locations:
if loc["room"] == self.roomw.resolve():
cleaning_locations.append(loc)
self.cleanup_locationsw.write(cleaning_locations)
# Show the cleanup list on screen
rospy.loginfo("Cleaning locations: {}".format(self.cleanup_locationsw.resolve()))
return

def setup_statemachine(robot, room):
def execute(self, userdata):
max_tries = 5
nr_of_tries = 0
count = 0

self.robot.head.look_at_standing_person(3)

while nr_of_tries < max_tries and not rospy.is_shutdown():
while not rospy.is_shutdown():
count += 1
self.robot.speech.speak("Which room should I clean for you?", block=True)
try:
speech_result = self.robot.hmi.query(description="",
grammar=challenge_knowledge.grammar,
target="T")
rospy.loginfo("sentence: {}".format(speech_result.sentence))
rospy.loginfo("semantics: {}".format(speech_result.semantics))
self.roomw.write(speech_result.sentence)
rospy.loginfo("roomw: {}".format(self.roomw.resolve()))
break
except (hmi.TimeoutException, hmi.GoalNotSucceededException) as e:
if count < 5:
self.robot.speech.speak(random.choice(["I'm sorry, can you repeat",
"Please repeat, I didn't hear you",
"I didn't get that can you repeat it",
"Please speak up, as I didn't hear you"]))
else:
self.robot.speech.speak("I am sorry but I cannot understand you. I will quit now", block=False)
self.robot.head.cancel_goal()
return "failed"

sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
try:
# Now: confirm
# self.roomw.write(speech_result.sentence)
self.robot.speech.speak("I understood that the {} should be cleaned "
"is this correct?".format(speech_result.sentence))
except:
continue

# Show object locations in designated room
cleaning_locations = collect_cleanup_entities(room)
rospy.loginfo("Cleaning locations: {}".format(cleaning_locations))
try:
speech_result = self.robot.hmi.query(description="Is this correct?", grammar="T[True] -> yes;"
"T[False] -> no",
target="T")
except TimeoutException:
return "failed"

with sm:
self.robot.head.cancel_goal()
self.robot.speech.speak("Ok, I will clean the {}".format(self.roomw.resolve()), block=False)
self.collect_cleanup_locations()


return "done"

nr_of_tries += 1

# smach.StateMachine.add( "INITIALIZE",
# robot_smach_states.Initialize(robot),
# transitions={ "initialized" :"SET_INIT", "abort" :"Aborted"})
def setup_statemachine(robot):

sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
# Designators
# Room to search through
roomr = ds.VariableDesignator('kitchen', resolve_type=str)
roomw =roomr.writeable
# Answer given by operator
answerr = ds.VariableDesignator('yes', resolve_type=str)
answerw = ds.VariableWriter(answerr)

# Cleanup location as defined in local knowledge
cleanup_locationsr = ds.VariableDesignator([{'1':'2', '3':'4'}])
cleanup_locationsw = cleanup_locationsr.writeable
location_des = ds.VariableDesignator(resolve_type=dict)

with sm:
# Somehow, the next commented states do not work properly.....
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
robot_smach_states.StartChallengeRobust(robot,challenge_knowledge.starting_point),
transitions={"Done": "VERIFY",
"Aborted": "VERIFY",
"Failed": "VERIFY"})

# smach.StateMachine.add("SAY_START_CHALLENGE",
# robot_smach_states.Say(robot, "Lets get ready to clean some rooms", block=True),
# transitions={'spoken': 'VERIFY'})

# smach.StateMachine.add("SET_INIT",
# SetInitialPose(robot, challenge_knowledge.starting_point),
# transitions={'done': 'VERIFY',
# 'preempted': 'Aborted',
# 'error': 'VERIFY'})
# smach.StateMachine.add("START_CHALLENGE_ROBUST",
# robot_smach_states.StartChallengeRobust(robot,challenge_knowledge.initial_pose),
# transitions={"Done": "GO_TO_START",
# "Aborted": "GO_TO_START",
# "Failed": "GO_TO_START"})
# smach.StateMachine.add("GO_TO_START",
# robot_smach_states.NavigateToWaypoint(robot=robot,
# waypoint_designator=EntityByIdDesignator(robot=robot,
# id=challenge_knowledge.starting_point),
# radius=0.3),
# transitions={"arrived": "INQUIRE_ROOM",
# "unreachable": "INQUIRE_ROOM",
# "goal_not_defined": "INQUIRE_ROOM"})


# The next two states 'teleport' the robot from the initial_pose to the challenge starting point
# in the arena. These states replace the two states above, which do not work correctly.

smach.StateMachine.add("INITIALIZE",
robot_smach_states.Initialize(robot),
transitions={"initialized": "SET_INITIAL_POSE",
"abort": "Aborted"})
smach.StateMachine.add("SET_INITIAL_POSE",
robot_smach_states.SetInitialPose(robot,challenge_knowledge.starting_point),
transitions={"done": "INQUIRE_ROOM",
"preempted": "Aborted",
"error": "INQUIRE_ROOM"})

smach.StateMachine.add("INQUIRE_ROOM",
AskWhichRoomToClean(robot, roomw, answerw, cleanup_locationsw),
transitions={"done": "VERIFY",
"failed": "INQUIRE_ROOM"})

smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={"done": "SAY_START_CHALLENGE", "failed" : "SAY_KNOWLEDGE_NOT_COMPLETE"})
VerifyWorldModelInfo(robot),
transitions={"done": "SAY_START_CHALLENGE", "failed": "SAY_KNOWLEDGE_NOT_COMPLETE"})

smach.StateMachine.add('SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, ["My knowledge of the world is not complete!",
"Please give me some more information!"], block=False),
transitions={"spoken": "Aborted"})
robot_smach_states.Say(robot, ["My knowledge of the world is not complete!",
"Please give me some more information!"], block=False),
transitions={"spoken": "Aborted"})

smach.StateMachine.add('SAY_START_CHALLENGE',
robot_smach_states.Say(robot, ["Starting the cleanup challenge",
"What a mess here, let's clean this room!",
"Let's see if I can find some garbage here",
"All I want to do is clean this mess up!"], block=False),
transitions={"spoken": "INSPECT_0"})
transitions={"spoken": "ITERATE_NEXT_LOC"})

for i, place in enumerate(cleaning_locations):
next_state = "INSPECT_%d" % (i + 1) if i + 1 < len(cleaning_locations) else "RETURN_TO_OPERATOR"
# Here the designator cleanup_locationsr has to be iterated over to visit all locations of the room (see designator_iterator.py)
# How is this to be done?
smach.StateMachine.add('ITERATE_NEXT_LOC',
robot_smach_states.IterateDesignator(cleanup_locationsr, location_des.writeable),
transitions={"next": "INSPECT",
"stop_iteration": "RETURN_TO_OPERATOR"})

smach.StateMachine.add("INSPECT_%d" % i,
CleanInspect(robot, place["name"], place["room"], place["navigate_area"],
place["segment_areas"]),
transitions={"done": next_state})
smach.StateMachine.add("INSPECT",
CleanInspect(robot, location_des),
transitions={"done": "ITERATE_NEXT_LOC"})

smach.StateMachine.add("RETURN_TO_OPERATOR",
robot_smach_states.NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
waypoint_designator=ds.EntityByIdDesignator(robot=robot,
id=challenge_knowledge.starting_point),
radius=0.3),
transitions={"arrived": "SAY_CLEANED_ROOM",
"unreachable": "SAY_CLEANED_ROOM",
"goal_not_defined": "SAY_CLEANED_ROOM"})

smach.StateMachine.add('SAY_CLEANED_ROOM',
robot_smach_states.Say(robot, ["I successfully cleaned the {}!".format(room),
robot_smach_states.Say(robot, ["I successfully cleaned the {}!".format(roomr),
"All done. Am I a good robot now?",
"There, I cleaned up your mess, are you happy now!"], block=False),
transitions={"spoken": "Done"})

return sm

def confirm(robot):
cgrammar = """
C[P] -> A[P]
A['yes'] -> yes
A['no'] -> no
"""
try:
speech_result = robot.hmi.query(description="Is this correct?", grammar="T[True] -> yes;"
"T[False] -> no", target="T")
except TimeoutException:
return False

return speech_result.semantics

def ask_which_room_to_clean(robot):
max_tries = 5
nr_of_tries = 0
count = 0

robot.head.look_at_standing_person(3)

while nr_of_tries < max_tries and not rospy.is_shutdown():
while not rospy.is_shutdown():
count += 1
robot.speech.speak("Which room should I clean for you?", block=True)
try:
speech_result = robot.hmi.query(description="",
grammar=challenge_knowledge.grammar,
target="T")
rospy.loginfo("sentence: {}".format(speech_result.sentence))
rospy.loginfo("semantics: {}".format(speech_result.semantics))
break
except (hmi.TimeoutException, hmi.GoalNotSucceededException) as e:
if count < 5:
robot.speech.speak(random.choice(["I'm sorry, can you repeat",
"Please repeat, I didn't hear you",
"I didn't get that can you repeat it",
"Please speak up, as I didn't hear you"]))
else:
robot.speech.speak("I am sorry but I cannot understand you. I will quit now", block=False)
robot.head.cancel_goal()
return "failed"

try:
# Now: confirm
robot.speech.speak("I understood that the {} should be cleaned "
"is this correct?".format(speech_result.sentence))
except:
continue

if confirm(robot):
robot.head.cancel_goal()
robot.speech.speak("Ok, I will clean the {}".format(speech_result.sentence), block=False)
return speech_result.sentence

nr_of_tries += 1



def main():
rospy.init_node('cleanup_challenge')

skip = rospy.get_param('~skip', False)
robot_name = rospy.get_param('~robot_name')

if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
raise ValueError('unknown robot')

robot = Robot()

# Wait for door, enter arena
# skip is defined and set in the launchfile. (TRUE)
if not skip:
#s = StartChallengeRobust(robot, challenge_knowledge.initial_pose)
#s.execute()

robot.speech.speak("Moving to the meeting point.", block=False)
nwc = robot_smach_states.NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=challenge_knowledge.starting_point),
radius=0.3)
nwc.execute()

room = ask_which_room_to_clean(robot)

# start execution of challenge
sm_args = [room]
robot_smach_states.util.startup(setup_statemachine, challenge_name="cleanup", statemachine_args=sm_args)
# For testing purposes, pick a default room
room = 'bedroom'

robot_smach_states.util.startup(setup_statemachine, challenge_name="cleanup")


if __name__ == '__main__':
Expand Down

0 comments on commit 13ae085

Please sign in to comment.