Skip to content

Commit

Permalink
Merge pull request #978 from tue-robotics/docs
Browse files Browse the repository at this point in the history 
Docs
  • Loading branch information
@LarsJanssenTUe
LarsJanssenTUe committed Feb 18, 2020
2 parents 872f2ca + 2715d83 commit 4442914
Show file tree
Hide file tree
Showing 103 changed files with 639 additions and 1,125 deletions.
8 changes: 8 additions & 0 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
[submodule "robot_smach_states/docs"]
path = robot_smach_states/docs
url = https://github.com/tue-robotics/tue_documentation_python.git
branch = master
[submodule "robot_skills/docs"]
path = robot_skills/docs
url = https://github.com/tue-robotics/tue_documentation_python.git
branch = master
24 changes: 14 additions & 10 deletions challenge_cleanup/src/challenge_cleanup/clean_inspect.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
import smach

import robot_smach_states as states
from robot_smach_states.designator_iterator import IterateDesignator
from robot_smach_states.human_interaction import Say
from robot_smach_states.navigation import NavigateToSymbolic
from robot_smach_states.rise import RiseForInspect
from robot_smach_states.util.designators import EdEntityDesignator, VariableDesignator, ValueByKeyDesignator
from robot_smach_states.world_model import SegmentObjects
from handle_detected_entities import HandleDetectedEntities
from robot_skills.classification_result import ClassificationResult

Expand Down Expand Up @@ -32,34 +36,34 @@ def __init__(self, robot, location_des):

with self:
smach.StateMachine.add('ITERATE_NEXT_AREA',
states.IterateDesignator(segment_areas, segment_area.writeable),
IterateDesignator(segment_areas, segment_area.writeable),
transitions={"next": 'INSPECT',
"stop_iteration": "done"})

# Segment the area and handle segmented objects for the specified navigation area
# for i, segment_area in enumerate(segment_areas):

smach.StateMachine.add('INSPECT',
states.NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des),
NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des),
transitions={'arrived': 'SEGMENT_SAY',
'unreachable': "SAY_UNREACHABLE",
'goal_not_defined': "SAY_UNREACHABLE"})

smach.StateMachine.add("SEGMENT_SAY",
states.Say(robot, "Looking {area} the {entity}", area=segment_area,
entity=location_id_des, block=False),
Say(robot, "Looking {area} the {entity}", area=segment_area,
entity=location_id_des, block=False),
transitions={"spoken": "RISE"})

smach.StateMachine.add('RISE', states.RiseForInspect(robot, e_des, segment_area),
smach.StateMachine.add('RISE', RiseForInspect(robot, e_des, segment_area),
transitions={'succeeded': 'SEGMENT',
'failed': 'SEGMENT'})

smach.StateMachine.add('SEGMENT', states.SegmentObjects(robot, e_classifications_des.writeable, e_des,
segment_area),
smach.StateMachine.add('SEGMENT', SegmentObjects(robot, e_classifications_des.writeable, e_des,
segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES"})

smach.StateMachine.add("SAY_UNREACHABLE", states.Say(robot, "I failed to inspect the {furn}",
furn=location_id_des, block=True),
smach.StateMachine.add("SAY_UNREACHABLE", Say(robot, "I failed to inspect the {furn}",
furn=location_id_des, block=True),
transitions={"spoken": "done"})

smach.StateMachine.add("HANDLE_DETECTED_ENTITIES",
Expand Down
59 changes: 28 additions & 31 deletions challenge_cleanup/src/challenge_cleanup/cleanup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,10 @@


import hmi
import robot_smach_states
from robot_smach_states.human_interaction.human_interaction import AskYesNo, HearOptionsExtra, Say
from robot_smach_states.designator_iterator import IterateDesignator
from robot_smach_states.navigation.navigate_to_waypoint import NavigateToWaypoint
from robot_smach_states.startup import StartChallengeRobust
import robot_smach_states.util.designators as ds
from clean_inspect import CleanInspect

Expand Down Expand Up @@ -125,18 +128,16 @@ def write_room(ud, des_read, des_write):
return 'done'

with self:
smach.StateMachine.add("ASK_WHICH_ROOM", robot_smach_states.Say(robot, "Which room should I clean for you?",
block=True),
smach.StateMachine.add("ASK_WHICH_ROOM", Say(robot, "Which room should I clean for you?", block=True),
transitions={"spoken": "HEAR_ROOM"})
smach.StateMachine.add("HEAR_ROOM", robot_smach_states.HearOptionsExtra(robot, room_grammar,
ds.writeable(hmi_result_des)),
smach.StateMachine.add("HEAR_ROOM", HearOptionsExtra(robot, room_grammar, ds.writeable(hmi_result_des)),
transitions={"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHICH_ROOM"})
smach.StateMachine.add("SAY_HEARD_CORRECT", robot_smach_states.Say(
robot, "I understood that the {room} should be cleaned, is this correct?", room=room_name_des,
block=True),
smach.StateMachine.add("SAY_HEARD_CORRECT",
Say(robot, "I understood that the {room} should be cleaned, is this correct?",
room=room_name_des, block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT", robot_smach_states.AskYesNo(robot),
smach.StateMachine.add("HEAR_CORRECT", AskYesNo(robot),
transitions={"yes": "FILL_LOCATIONS",
"no": "ASK_WHICH_ROOM",
"no_result": "ASK_WHICH_ROOM"})
Expand All @@ -162,23 +163,21 @@ def setup_statemachine(robot):

with sm:
smach.StateMachine.add("START_ROBUST",
robot_smach_states.StartChallengeRobust(robot, challenge_knowledge.starting_point),
StartChallengeRobust(robot, challenge_knowledge.starting_point),
transitions={"Done": "GO_TO_WAITING_POINT",
"Aborted": "GO_TO_WAITING_POINT",
"Failed": "GO_TO_WAITING_POINT"})

smach.StateMachine.add(
"GO_TO_WAITING_POINT",
robot_smach_states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot,
challenge_knowledge.waiting_point)),
NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, challenge_knowledge.waiting_point)),
transitions={"arrived": "INQUIRE_ROOM",
"unreachable": "GO_TO_WAITING_POINT1",
"goal_not_defined": "GO_TO_WAITING_POINT1"})

smach.StateMachine.add(
"GO_TO_WAITING_POINT1",
robot_smach_states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot,
challenge_knowledge.waiting_point),
NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, challenge_knowledge.waiting_point),
radius=0.3),
transitions={"arrived": "INQUIRE_ROOM",
"unreachable": "INQUIRE_ROOM",
Expand All @@ -193,41 +192,39 @@ def setup_statemachine(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),
smach.StateMachine.add('SAY_KNOWLEDGE_NOT_COMPLETE', 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),
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": "ITERATE_NEXT_LOC"})

smach.StateMachine.add('ITERATE_NEXT_LOC',
robot_smach_states.IterateDesignator(cleanup_locationsr, location_des.writeable),
IterateDesignator(cleanup_locationsr, location_des.writeable),
transitions={"next": "INSPECT",
"stop_iteration": "RETURN_TO_OPERATOR"})

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=ds.EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.starting_point),
radius=0.3),
NavigateToWaypoint(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 {room}!",
"All done in the {room}. Am I a good robot now?",
"There, I cleaned up your mess in the {room}, are you happy now!"],
Say(robot, ["I successfully cleaned the {room}!",
"All done in the {room}. Am I a good robot now?",
"There, I cleaned up your mess in the {room}, are you happy now!"],
room=roomr, block=False),
transitions={"spoken": "Done"})

Expand Down
26 changes: 13 additions & 13 deletions challenge_cleanup/src/challenge_cleanup/operator_cleanup.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
import smach
import robot_smach_states
import rospy

from robot_smach_states.human_interaction import Say

from PIL import Image
import cStringIO as StringIO
import numpy as np
Expand All @@ -11,7 +12,7 @@
from timeout import Timeout


def _get_cropped_image_from_info(info):
def _get_cropped_image_from_info(info):

if len(info.measurement_image_unmasked) == 0:
rospy.logerr("Received empty image from ED. This should not happen")
Expand All @@ -36,7 +37,7 @@ def _get_cropped_image_from_info(info):
image_data_new = image_data[cropBox[0]:cropBox[1]+1, cropBox[2]:cropBox[3]+1 , :]

cropped_image = Image.fromarray(image_data_new)
except:
except Exception:
rospy.logerr("Could not crop image, I will use the original image as cropped image")
cropped_image = image

Expand All @@ -60,7 +61,7 @@ def _request_with_timeout(self, img_msg, timeout):
timeout_function = Timeout(self._request, timeout)
return timeout_function(img_msg)

def execute(self, userdata):
def execute(self, userdata=None):
e = self._selected_entity_designator.resolve()

# If we have cached it, clean it up
Expand Down Expand Up @@ -130,21 +131,20 @@ def __init__(self, robot, selected_entity_designator, location_id, segment_area)
with self:

smach.StateMachine.add('SAY_CONTACTING_OPERATOR',
robot_smach_states.Say(robot, sentences, block=True),
Say(robot, sentences, block=True),
transitions={"spoken": "OPERATOR_FEEDBACK"})

smach.StateMachine.add('OPERATOR_FEEDBACK',
OperatorFeedback(robot, selected_entity_designator),
transitions={"cleanup": "SAY_CLEANUP", "no_cleanup": "SAY_NO_CLEANUP"})

smach.StateMachine.add('SAY_CLEANUP',
robot_smach_states.Say(robot, ["Ok, I will cleanup the object",
"That's ok!",
"As you wish"], block=True),
transitions={"spoken": "cleanup"})
Say(robot, ["Ok, I will cleanup the object",
"That's ok!",
"As you wish"], block=True),
transitions={"spoken": "cleanup"})

smach.StateMachine.add('SAY_NO_CLEANUP',
robot_smach_states.Say(robot, ["Ok, I will leave the object here",
"That's ok! I will continue",
"Now I know that this object is not trash"], block=True),
transitions={"spoken": "no_cleanup"})
Say(robot, ["Ok, I will leave the object here", "That's ok! I will continue",
"Now I know that this object is not trash"], block=True),
transitions={"spoken": "no_cleanup"})

0 comments on commit 4442914

Please sign in to comment.