def run(self):
self.command_center = CommandCenter(self.robot)
self.knowledge = load_knowledge('challenge_final')
self.command_center.set_grammar(
os.path.dirname(sys.argv[0]) + "/grammar.fcfg", self.knowledge)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
print "GOT HERE!"
self.command_center.register_action("take-order", self.take_order)
self.command_center.register_action("prepare", self.prepare)
self.command_center.register_action("serve", self.serve)
self.command_center.register_action("check", self.check)
self.command_center.register_action("come-in", self.come_in)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
sentences = ["What can I do for you?", "Can I do something?"]
rospy.loginfo("Ready, waiting for trigger!")
while not rospy.is_shutdown():
command_semantics = None
if self.sentence:
command_semantics = self.command_center.parse_command(
self.sentence)
if not command_semantics:
self.robot.speech.speak("I cannot parse \"{}\"".format(
self.sentence))
self.sentence = None
elif self.do_listen_command:
res = self.command_center.request_command(
ask_confirmation=True,
ask_missing_info=False,
sentences=sentences,
n_tries=2)
if not res:
self.robot.speech.speak("I did not understand")
else:
(command_words, command_semantics) = res
self.do_listen_command = False
else:
time.sleep(0.1)
if command_semantics:
print "Command semantics: {}".format(command_semantics)
self.command_center.execute_command(command_semantics)
def entities_from_description(robot, entity_description, list_of_entity_ids=None ):
'''
Query entities and return those that satisfy the given description
@param robot: The robot object
@param entity_descr: A dict that contains a 'type' field
@param list_of_entity_ids: A list of entity ids to choose from (for example a result of a segment)
@return: entities
entities - list of entities that fulfill the description
(each element has type Entity)
'''
knowledge = load_knowledge('common')
if not isinstance(list_of_entity_ids, list):
return []
if not isinstance(entity_description, dict):
return []
if 'type' not in entity_description:
return []
# Get all entities from the world model
entities = robot.ed.get_entities()
# TODO: hack because ed maintains all labels that were ever assigned to an entity in the .types field
if entity_description['type'] == 'person':
entities = [e for e in entities if e.is_a('possible_human')]
# Remove the segmented entities from the inspection
for id in list_of_entity_ids:
robot.ed.update_entity(id=id, action='remove')
else:
# Select entities based on the description (only type for now)
try:
if entity_description['type'] in knowledge.object_categories:
entities = [e for e in entities if knowledge.get_object_category(e.type) == entity_description['type']]
else:
entities = [e for e in entities if e.type == entity_description['type']]
except:
entities = [e for e in entities if e.type == entity_description['type']]
# If we have a list of entities to choose from, select based on that list
if list_of_entity_ids:
entities = [e for e in entities if e.id in list_of_entity_ids]
# Sort entities by distance
robot_location = robot.base.get_location()
robot_pos = robot_location.frame.p
entities = sorted(entities, key=lambda entity: entity.distance_to_2d(robot_pos))
return entities
def __init__(self, gpsr_category=1):
sentence_data_file = os.path.dirname(sys.argv[0]) + ("/../test/sentences/sentences_cat_{}.txt".format(gpsr_category))
self.example_commands = []
with open(sentence_data_file) as f:
for line in f:
line = line.strip()
if line:
self.example_commands += [line]
# Seed random
random.seed(datetime.now())
self.knowledge = load_knowledge('challenge_gpsr')
def test_grammar():
# Export a (default) robot env. This is necessary because the action server
# loads knowledge on construction of actions.
# It is desirable to improve this in the future.
os.environ["ROBOT_ENV"] = "robotics_testlabs"
knowledge = load_knowledge('challenge_demo')
# Construct a Mockbot object and add a number of static entities
robot = Mockbot()
robot.ed._static_entities = {
"couch_table": from_entity_info(EntityInfo(id="couch_table")),
"operator": from_entity_info(EntityInfo(id="operator")),
}
robot.ed._dynamic_entities = dict()
test_grammar(robot, knowledge.grammar, knowledge.grammar_target)
def __init__(self, gpsr_category=1):
sentence_data_file = os.path.dirname(sys.argv[0]) + (
"/../test/sentences/sentences_cat_{}.txt".format(gpsr_category))
self.example_commands = []
with open(sentence_data_file) as f:
for line in f:
line = line.strip()
if line:
self.example_commands += [line]
# Seed random
random.seed(datetime.now())
self.knowledge = load_knowledge('challenge_gpsr')
def entities_from_description(robot, entity_description, list_of_entity_ids=None):
'''
Query entities and return those that satisfy the given description
@param robot: The robot object
@param entity_description: A dict that contains a 'type' field
@param list_of_entity_ids: A list of entity ids to choose from (for example a result of a segment)
@return: entities
entities - list of entities that fulfill the description
(each element has type Entity)
'''
knowledge = load_knowledge('common')
list_of_entity_ids = list_of_entity_ids if list_of_entity_ids is not None else []
assert isinstance(list_of_entity_ids, list), "Input should be a list"
assert isinstance(entity_description, dict), "Entity description should be a dict"
assert 'type' in entity_description or 'category' in entity_description
# Get all entities from the world model
entities = robot.ed.get_entities()
# Select entities based on the description
if entity_description.get('type'):
entities = [e for e in entities if e.type == entity_description['type']]
elif entity_description.get('category'):
entities = [e for e in entities if knowledge.get_object_category(e.type) == entity_description['category']]
else:
return []
# If we have a list of entities to choose from, select based on that list
if list_of_entity_ids:
entities = [e for e in entities if e.id in list_of_entity_ids]
# Sort entities by distance
robot_location = robot.base.get_location()
robot_pos = robot_location.frame.p
entities = sorted(entities, key=lambda entity: entity.distance_to_2d(robot_pos))
return entities
def main():
num_sentences = 1
if len(sys.argv) > 1:
num_sentences = int(sys.argv[1])
command_center = CommandCenter()
challenge_knowledge = load_knowledge('challenge_gpsr')
command_center.set_grammar(
os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
req_spec = command_center.command_recognizer.grammar_string
req_choices = command_center.command_recognizer.choices
for i in range(0, num_sentences):
# Copy request
example = "(%s)" % req_spec
# Pick random group if available
while re.search('\([^\)]+\)', example):
options = re.findall('\([^\(\)]+\)', example)
for option in options:
example = example.replace(
option, random.choice(option[1:-1].split("|")), 1)
# Fetch all the residual choices
choices = re.findall("<([^<>]+)>", example)
# Parse the choices in the ending result :)
for c in choices:
for req_c in req_choices:
if req_c == c:
value = random.choice(req_choices[req_c])
example = example.replace("<%s>" % c, value)
print example
from ed_msgs.msg import EntityInfo
from smach_ros import SimpleActionState
from collections import namedtuple
from dragonfly_speech_recognition.srv import GetSpeechResponse
from robot_smach_states.util.designators import *
from robot_smach_states.human_interaction.human_interaction import HearOptionsExtra
from robot_smach_states import Grab
from robocup_knowledge import load_knowledge
from robot_skills.util import transformations
from robot_skills.arms import Arm
# ----------------------------------------------------------------------------------------------------
common_knowledge = load_knowledge("common")
challenge_knowledge = load_knowledge("challenge_person_recognition")
OBJECT_TYPES = challenge_knowledge.object_types
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints("[Challenge Test] ")
# ----------------------------------------------------------------------------------------------------
# OBSELETE! BUT MIGHT BE USEFULL
# class PointAtOperator(smach.State):
# def __init__(self, robot):
# smach.State.__init__(self, outcomes=['done'])
# self.robot = robot
import rospy
import robot_smach_states as states
import robot_smach_states.util.designators as ds
import smach
from hmi import HMIResult
from robocup_knowledge import load_knowledge
from robot_skills.util.entity import Entity
from challenge_receptionist.find_empty_seat import FindEmptySeat
from challenge_receptionist.learn_guest import LearnGuest
from challenge_receptionist.introduce_guest import IntroduceGuest
challenge_knowledge = load_knowledge('challenge_receptionist')
class HandleSingleGuest(smach.StateMachine):
def __init__(self, robot, assume_john):
"""
:param robot:
:param assume_john: bool indicating that John (the homeowner) is already there.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
guest_entity_des = ds.VariableDesignator(resolve_type=Entity,
name='guest_entity')
guest_name_des = ds.VariableDesignator('guest 1', name='guest_name')
#! /usr/bin/python
import sys
import rospy
import random
from robocup_knowledge import load_knowledge
from hmi import TimeoutException
knowledge = load_knowledge('challenge_gpsr')
def main():
rospy.init_node("test_gpsr_recognition", anonymous=True)
random.seed()
if "sergio" in str(sys.argv):
from robot_skills.sergio import Sergio as Robot
else:
from robot_skills.amigo import Amigo as Robot
robot = Robot()
while True:
description = random.choice(["Ask me a question", "Ask me something", "What do you want?", "At your service"])
robot.speech.speak(description)
try:
sentence, semantics = robot.hmi.query(description, knowledge.grammar, knowledge.grammar_target)
except TimeoutException as e:
robot.speech.speak("I did not hear anything, please try again")
else:
robot.speech.speak("I heard {}".format(sentence))
import smach
# TU/e Robotics
from robocup_knowledge import load_knowledge
import robot_smach_states as states
from robot_smach_states.util.startup import startup
import robot_smach_states.util.designators as ds
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, analyse_designators
# Set the table
from challenge_set_a_table_states.fetch_command import HearFetchCommand, GetBreakfastOrder
from challenge_set_a_table_states.manipulate_machine import ManipulateMachine
from challenge_set_a_table_states.clear_manipulate_machine import ClearManipulateMachine
# Load all knowledge
knowledge = load_knowledge('challenge_set_a_table')
INTERMEDIATE_1 = knowledge.intermediate_1
STARTING_POINT = knowledge.starting_point
class ChallengeSetATable(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
eegpsr = rospy.get_param('~eegpsr', False)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
if time_limit == 0:
time_limit = 999
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
rospy.loginfo("[GPSR] time_limit = {}".format(time_limit))
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
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()
if eegpsr:
knowledge = load_knowledge('challenge_eegpsr')
else:
knowledge = load_knowledge('challenge_gpsr')
conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar, knowledge.grammar_target, knowledge)
conversation_engine.test = test
conversation_engine.skip = skip
conversation_engine.tasks_to_be_done = no_of_tasks
conversation_engine.time_limit = time_limit
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!", block=False)
if restart:
robot.speech.speak("Performing a restart. So sorry about that last time!", block=False)
conversation_engine._start_wait_for_command(knowledge.grammar, knowledge.grammar_target)
rospy.spin()
#!/usr/bin/python
import math
import rospy
import smach
import datetime
import robot_smach_states as states
import robot_smach_states.util.designators as ds
from robot_skills.arms import GripperState
from robocup_knowledge import load_knowledge
from challenge_hmc_functions import hmc_states
from robot_skills.util import kdl_conversions
from robot_skills.util.entity import Entity
challenge_knowledge = load_knowledge('challenge_help_me_carry')
print "=============================================="
print "== CHALLENGE HELP ME CARRY =="
print "=============================================="
class ChallengeHelpMeCarry(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
#!/usr/bin/env python
import rospy
import smach
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, analyse_designators
import robot_smach_states as states
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_rips')
STARTING_POINT = challenge_knowledge.starting_point
INTERMEDIATE_1 = challenge_knowledge.intermediate_1
INTERMEDIATE_2 = challenge_knowledge.intermediate_2
INTERMEDIATE_3 = challenge_knowledge.intermediate_3
EXIT_1 = challenge_knowledge.exit_1
EXIT_2 = challenge_knowledge.exit_2
EXIT_3 = challenge_knowledge.exit_3
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(robot, STARTING_POINT),
transitions={"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"})
#!/usr/bin/python
# ROS
import rospy
import smach
# TU/e Robotics
import robot_smach_states as states
from robot_smach_states.human_interaction.answer_questions import HearAndAnswerQuestions
from robot_smach_states.util.startup import startup
from robocup_knowledge import load_knowledge
knowledge = load_knowledge('challenge_spr')
common_knowledge = load_knowledge('common')
class TestRiddleGame(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
self.userdata.crowd_data = {
"males": 3,
"men": 2,
"females": 5,
"women": 3,
"children": 3,
"boys": 1,
"girls": 2,
"adults": 5,
"elders": 1,
"crowd_size": 8
}
def main():
rospy.init_node("ee_gpsr")
parser = argparse.ArgumentParser()
parser.add_argument('robot', help='Robot name')
parser.add_argument('--once', action='store_true', help='Turn on infinite loop')
parser.add_argument('--skip', action='store_true', help='Skip enter/exit')
parser.add_argument('sentence', nargs='*', help='Optional sentence')
args = parser.parse_args()
rospy.loginfo('args: %s', args)
mock_sentence = " ".join([word for word in args.sentence if word[0] != '_'])
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if args.robot == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif args.robot == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
# Sleep for 1 second to make sure everything is connected
time.sleep(1)
command_center = CommandCenter(robot)
challenge_knowledge = load_knowledge('challenge_eegpsr')
command_center.set_grammar(os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not args.skip:
# Wait for door, enter arena
s = StartChallengeRobust(robot, challenge_knowledge.initial_pose)
s.execute()
# Move to the start location
nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3)
nwc.execute()
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
sentence = " ".join([word for word in args.sentence if word[0] != '_'])
first = True
if sentence:
semantics = command_center.parse_command(sentence)
if not semantics:
rospy.logerr("Cannot parse \"{}\"".format(sentence))
return
command_center.execute_command(semantics)
else:
while True:
if first:
# First sentence robot says
sentences = ["Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping."]
else:
# Sentence robot says after completing a task
sentences = ["Hello there, you look lovely! I'm here to take a new order, but wait for the ping!"]
# These sentences are for when the first try fails
# (Robot says "Do you want me to ...?", you say "No", then robot says sentence below)
sentences += [
"I'm so sorry! Can you please speak louder and slower? And wait for the ping!",
"Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!",
"You and I have communication issues. Speak up! Tell me what you want, but wait for the ping"
]
hey_robot_wait_forever(robot)
res = command_center.request_command(ask_confirmation=True, ask_missing_info=False, timeout=600, sentences=sentences)
if not res:
continue
first = False
(sentence, semantics) = res
if not command_center.execute_command(semantics):
robot.speech.speak("I am truly sorry, let's try this again")
if args.once:
break
if not args.skip:
nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3)
nwc.execute()
#!/usr/bin/env python
import rospy
from robocup_knowledge import load_knowledge
from robot_smach_states.util.startup import startup
from robot_smach_states.util.designators import EntityByIdDesignator
from empty_shelf_designator import EmptyShelfDesignator
challenge_knowledge = load_knowledge('challenge_manipulation')
USE_SLAM = True # Indicates whether or not to use SLAM for localization
if USE_SLAM:
CABINET = challenge_knowledge.cabinet_slam
else:
CABINET = challenge_knowledge.cabinet_amcl
PLACE_SHELF = challenge_knowledge.place_shelf
'''
def main(robot):
place_position = LockingDesignator(
EmptyShelfDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position")
@smach.cb_interface(outcomes=['done'])
def lock_pp(ud, x, y, z):
place_position.lock()
return 'success'
@smach.cb_interface(outcomes=['done'])
def unlock_pp(ud, x, y, z):
place_position.unlock()
#!/usr/bin/python
import roslib;
import rospy
import smach
import sys
import time
from cb_planner_msgs_srvs.msg import PositionConstraint
from robot_smach_states.util.designators import VariableDesignator, EdEntityDesignator, EntityByIdDesignator, analyse_designators
import robot_smach_states as states
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_navigation')
print "=============================================="
print "== CHALLENGE NAVIGATION =="
print "=============================================="
class checkTimeOut(smach.State):
def __init__(self, robot, time_out_seconds):
smach.State.__init__(self, outcomes=["not_yet", "time_out"])
self.robot = robot
self.time_out_seconds = time_out_seconds
self.start = None
self.last_say = None
self.turn = -1
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
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()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn(
"[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot, id=location_id),
radius=0.3)
nwc.execute()
# System
import random
import traceback
# ROS
import rospy
import smach
import sys
# TU/e Robotics
import robot_smach_states as states
from robot_smach_states.util.startup import startup
from hmi import TimeoutException
from robocup_knowledge import load_knowledge
knowledge = load_knowledge('challenge_spr')
common_knowledge = load_knowledge('common')
##############################################################################
#
# Default parameters:
#
##############################################################################
DEFAULT_HEAR_TIME = 20.0
##############################################################################
#
# Main class:
#
##############################################################################
def main():
rospy.init_node("gpsr")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
test = rospy.get_param('~test_mode', False)
rospy.loginfo("[GPSR] Parameters:")
rospy.loginfo("[GPSR] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[GPSR] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[GPSR] running a restart")
if test:
rospy.loginfo("[GPSR] running in test mode")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_gpsr')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
while True:
# Navigate to the GPSR meeting point
if not skip and not finished:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
while True:
while True and not test:
try:
robot.hmi.query(description="",
grammar="T -> %s" % robot_name,
target="T")
except hmi.TimeoutException:
continue
else:
break
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
continue
if not test:
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no',
'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) #be sure lights are blue
robot.head.look_at_standing_person()
robot.leftArm.reset()
robot.leftArm.send_gripper_goal('close', 0.0)
robot.rightArm.reset()
robot.rightArm.send_gripper_goal('close', 0.0)
robot.torso.reset()
if task_result.succeeded:
# Keep track of the number of performed tasks
no_of_tasks_performed += 1
if no_of_tasks_performed >= no_of_tasks:
finished = True
# If we succeeded, we can say something optimistic after reporting to the operator
if no_of_tasks_performed == 1:
task_word = "task"
else:
task_word = "tasks"
report += " I performed {} {} so far, still going strong!".format(
no_of_tasks_performed, task_word)
if rospy.get_time() - start_time > 60 * 15:
finished = True
if finished and not skip:
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.exit_waypoint),
radius=0.3)
nwc.execute()
robot.speech.speak("Thank you very much, and goodbye!", block=True)
break
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
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()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!", block=False)
if restart:
robot.speech.speak("Performing a restart. So sorry about that last time!", block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped(base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(random.sample(knowledge.not_understood_sentences, 1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn("[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(robot=robot,
id=location_id),
radius=0.3)
nwc.execute()
#!/usr/bin/python
import re
import random
from robocup_knowledge import load_knowledge
data = load_knowledge('challenge_speech_recognition')
# Pick random group if available
while re.search('\([^\)]+\)', data.spec):
options = re.findall('\([^\(\)]+\)', data.spec)
for option in options:
data.spec = data.spec.replace(option,
random.choice(option[1:-1].split("|")),
1)
# Fetch all the residual choices
choices = re.findall("<([^<>]+)>", data.spec)
# Parse the choices in the ending result :)
for c in choices:
for k, v in data.choices.iteritems():
if k == c:
value = random.choice(v)
data.spec = data.spec.replace("<%s>" % c, value)
# Check if result is clean
if re.match(".*[<>\(\)\[\]]+.*", data.spec):
print "Not all choices could be resolved in the specification, residual result: '%s'"
else:
#!/usr/bin/python
import rospy
import smach
import sys
import robot_smach_states
from clean_inspect import CleanInspect
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('r5cop_demo')
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add( "START_CHALLENGE_ROBUST",
robot_smach_states.Initialize(robot),
transitions={"initialized": "SAY_START_CHALLENGE",
"abort": "Aborted"})
smach.StateMachine.add('SAY_START_CHALLENGE',
robot_smach_states.Say(robot, ["Starting R5COP Cooperative cleaning demonstrator",
"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),
#!/usr/bin/python
import roslib;
import rospy
import smach
import sys
import robot_smach_states as states
from robot_smach_states.util.designators import Designator, EdEntityDesignator
from robocup_knowledge import load_knowledge
data = load_knowledge('challenge_speech_recognition')
class HearQuestion(smach.State):
def __init__(self, robot, time_out=rospy.Duration(15)):
smach.State.__init__(self, outcomes=["answered"])
self.robot = robot
self.time_out = time_out
def execute(self, userdata=None):
self.robot.head.look_at_ground_in_front_of_robot(100)
res = self.robot.ears.recognize(spec=data.spec, choices=data.choices, time_out=self.time_out)
if not res:
self.robot.speech.speak("My ears are not working properly.")
if res:
if "question" in res.choices:
rospy.loginfo("Question was: '%s'?"%res.result)
self.robot.speech.speak("The answer is %s"%data.choice_answer_mapping[res.choices['question']])
else:
import rospy
import smach
import sys
import math
import time
from visualization_msgs.msg import Marker
import robot_smach_states as states
from robot_smach_states.util.startup import startup
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, analyse_designators
from robot_skills.util import transformations, msg_constructors
from robocup_knowledge import load_knowledge
knowledge = load_knowledge("challenge_following_and_guiding")
class StoreRobocupArena(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=["done"])
self._robot = robot
robot.base.local_planner.cancelCurrentPlan()
def execute(self, userdata=None):
self._robot.ed.update_entity(id="robocup_arena", posestamped=self._robot.base.get_location(), type="waypoint")
return "done"
class HeadStraight(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=["done"])
import rospy
import smach
# TU/e Robotics
from robocup_knowledge import load_knowledge
import robot_smach_states as states
from robot_smach_states.util.startup import startup
import robot_smach_states.util.designators as ds
# Set the table
from challenge_set_a_table_states.fetch_command import HearFetchCommand, GetBreakfastOrder
from challenge_set_a_table_states.manipulate_machine import ManipulateMachine, DefaultGrabDesignator
from challenge_set_a_table_states.clear_manipulate_machine import ClearManipulateMachine
# Load all knowledge
knowledge = load_knowledge('challenge_set_a_table')
class ChallengeSetATable(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
#!/usr/bin/python
import roslib
import rospy
import smach
import sys
import time
from cb_planner_msgs_srvs.msg import PositionConstraint
from robot_smach_states.util.designators import VariableDesignator, EdEntityDesignator, EntityByIdDesignator, analyse_designators
import robot_smach_states as states
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_navigation')
print "=============================================="
print "== CHALLENGE NAVIGATION =="
print "=============================================="
class checkTimeOut(smach.State):
def __init__(self, robot, time_out_seconds):
smach.State.__init__(self, outcomes=["not_yet", "time_out"])
self.robot = robot
self.time_out_seconds = time_out_seconds
self.start = None
self.last_say = None
self.turn = -1
def init_command_center(robot=None):
command_center = CommandCenter(robot)
challenge_knowledge = load_knowledge('challenge_eegpsr')
command_center.set_grammar(os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge)
return command_center
import smach
import rospy
import hmi
from robot_smach_states.human_interaction import AskYesNo, HearOptionsExtra, Say
from robot_smach_states.manipulation import ArmToJointConfig, Grab, Place
from robot_smach_states.navigation import NavigateToPlace, NavigateToRoom
from robot_smach_states.utility import WaitTime
from robot_smach_states.world_model import Inspect
from robot_skills.util.kdl_conversions import FrameStamped
from robot_skills.util.entity import Entity
from robot_skills import arms
import robot_smach_states.util.designators as ds
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_cleanup')
# ToDo: Location for trash depends on the room chosen: trash_bin (living) or trash_can (kitchen). Not handled yet.
class DropPoseDesignator(ds.Designator):
def __init__(self, robot, entity_des, drop_height, name=None):
super(DropPoseDesignator, self).__init__(resolve_type=FrameStamped,
name=name)
self._robot = robot
self._entity_des = entity_des
self._drop_height = drop_height
def _resolve(self):
# ToDo: Make this happen for the bin in the chosen room...
from robot_smach_states.util.startup import startup
# import designators
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, VariableDesignator, DeferToRuntime, analyse_designators
# import states from another file
import test_states as test_states
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# INITIALIZATIONS
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# load knowledge
common_knowledge = load_knowledge("common")
challenge_knowledge = load_knowledge("challenge_test")
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints("[Challenge Test] ")
personNameDes = VariableDesignator("", resolve_type=str)
objectsIDsDes = VariableDesignator([], resolve_type=[ClassificationResult])
containerResultDes = VariableDesignator(0, resolve_type=int)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# NAVIGATION_CONTAINER
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# TU/e
from robot_skills.util.kdl_conversions import FrameStamped
# RoboCup knowledge
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_storing_groceries')
# Inspection
CABINET = challenge_knowledge.cabinet_amcl
OBJECT_SHELVES = challenge_knowledge.object_shelves
OBJECT_TYPES = challenge_knowledge.object_types
DETECTED_OBJECTS_WITH_PROBS = [] # List with entities and types. This is used to write to PDF. # ToDo: no global?!
SEGMENTED_ENTITIES = [] # List with segmented entities such that we can also grasp unknown entities
CLASSIFICATION_THRESHOLD = 0.3 # Threshold for perception. If classification below threshold, the type is not added
# to the world model and further on considered unknown.
MAX_KNOWN_OBJECTS = 10 # Maximum number of known objects to store in the PDF
MAX_UNKNOWN_OBJECTS = 5 # Maximum number of unknown objects to store in the PDF
MIN_OBJECT_HEIGHT = 0.1
# List to skip: these won't be written to the pdf
SKIP_LIST = ["fork", "spoon", "chopsticks"]
# Grasping
GRAB_SURFACE = "on_top_of"
MIN_GRAB_OBJECT_HEIGHT = 0.075
MAX_GRAB_OBJECT_WIDTH = 0.15
# Debug
DEBUG = False
#!/usr/bin/python
import math
import smach
import robot_smach_states as states
from robot_smach_states.util.designators import check_type
from robot_skills.arms import Arm, GripperState
from hmi import TimeoutException
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_help_me_carry')
class WaitForOperatorCommand(smach.State):
"""
The robot waits for command from the operator
Possible commands are two types:
- commands as outcomes: each possible command is possible outcome
- commands as userdata: the command is passed to the next state
"""
def __init__(self, robot, possible_commands, commands_as_outcomes=False, commands_as_userdata=False, target=None):
self._robot = robot
self._possible_commands = possible_commands
self._commands_as_outcomes = commands_as_outcomes
self._commands_as_userdata = commands_as_userdata
self.target_destination = target
if commands_as_outcomes: # each possible command is a separate outcome
#!/usr/bin/python
import sys
import robot_smach_states
import rospy
import smach
import random
from hmi import TimeoutException
from robocup_knowledge import load_knowledge
from robot_smach_states.util.designators import EdEntityDesignator, VariableDesignator, EntityByIdDesignator
from clean_inspect import CleanInspect
challenge_knowledge = load_knowledge('challenge_open')
class VerifyWorldModelInfo(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=["failed", "done"])
self._robot = robot
def execute(self, userdata=None):
ids = [e.id for e in self._robot.ed.get_entities()]
if "trashbin" not in ids:
rospy.logwarn("trashbin not in world model")
return "failed"
for place in challenge_knowledge.inspection_places:
if place["entity_id"] not in ids:
rospy.logwarn("%s not in world model", place["entity_id"])
#
# \author Rein Appeldoorn
from smach import StateMachine
import robot_smach_states.util.designators as ds
from robot_skills.util.entity import Entity
from robot_smach_states import NavigateToWaypoint, StartChallengeRobust, Say, VariableDesignator,\
UnoccupiedArmDesignator
from robocup_knowledge import load_knowledge
from get_furniture_from_operator_pose import GetFurnitureFromOperatorPose
from identify_object import IdentifyObject
from inspect_furniture_entity import InspectFurniture
challenge_knowledge = load_knowledge('challenge_hand_me_that')
STARTING_POINT = challenge_knowledge.starting_point # Location where the challenge starts
HOME_LOCATION = challenge_knowledge.home_location # Location where the robot will go and look at the operator
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
furniture_designator = VariableDesignator(resolve_type=Entity)
entity_designator = VariableDesignator(resolve_type=Entity)
arm_designator = UnoccupiedArmDesignator(robot, {})
with state_machine:
# Intro
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT),
import smach
import sys
import math
import time
import signal
from visualization_msgs.msg import Marker
import robot_smach_states as states
from robot_smach_states.util.startup import startup
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, analyse_designators
from robot_skills.util import transformations, msg_constructors
from robocup_knowledge import load_knowledge
knowledge = load_knowledge("challenge_following_and_guiding")
import sys, select, termios, tty
class SetPlateCarryingPose(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=["done"])
self._robot = robot
def execute(self, userdata):
self._robot.rightArm._send_joint_trajectory(
[[0, .4, 0.5, 1.3, .1, 0.6, -0.6]])
self._robot.leftArm._send_joint_trajectory(
[[-1.2, -.1, .6, 2.2, 1.0, 0.9, 0]])
return 'done'
import smach
import sys
import math
import time
from visualization_msgs.msg import Marker
import robot_smach_states as states
from robot_smach_states.util.startup import startup
from robot_smach_states.util.designators import VariableDesignator, EdEntityDesignator, EntityByIdDesignator, analyse_designators
from robot_skills.util import transformations, msg_constructors
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import load_knowledge
common_knowledge = load_knowledge("common")
knowledge = load_knowledge("challenge_restaurant")
from visualize import visualize_location
tables = {1: "one", 2: "two", 3: "three"}
# ORDERS is filled with 2 keys: "beverage" and "combo".
# - Each value is a dictionary itself, with keys "location" and "name".
# - key "location" gets its value from the values in the tables-dictionary
# - key "name" gets the value of the order, e.g. "banana and apple"
ORDERS = {}
WAYPOINT_RADIUS = 0.2
class HearWhichTable(smach.State):
#!/usr/bin/env python
import rospy
from robocup_knowledge import load_knowledge
from robot_smach_states.util.startup import startup
from robot_smach_states.util.designators import EntityByIdDesignator
from empty_shelf_designator import EmptyShelfDesignator
challenge_knowledge = load_knowledge('challenge_manipulation')
USE_SLAM = True # Indicates whether or not to use SLAM for localization
if USE_SLAM:
CABINET = challenge_knowledge.cabinet_slam
else:
CABINET = challenge_knowledge.cabinet_amcl
PLACE_SHELF = challenge_knowledge.place_shelf
'''
def main(robot):
place_position = LockingDesignator(
EmptyShelfDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position")
@smach.cb_interface(outcomes=['done'])
def lock_pp(ud, x, y, z):
place_position.lock()
return 'success'
@smach.cb_interface(outcomes=['done'])
def unlock_pp(ud, x, y, z):
place_position.unlock()
return 'done'
from robot_skills.classification_result import ClassificationResult
from robocup_knowledge import load_knowledge
from robot_smach_states.util.startup import startup
# import designators
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, VariableDesignator, DeferToRuntime, analyse_designators
# import states from another file
import test_states as test_states
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# INITIALIZATIONS
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# load knowledge
common_knowledge = load_knowledge("common")
challenge_knowledge = load_knowledge("challenge_test")
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints(
"[Challenge Test] ")
personNameDes = VariableDesignator("", resolve_type=str)
objectsIDsDes = VariableDesignator([], resolve_type=[ClassificationResult])
containerResultDes = VariableDesignator(0, resolve_type=int)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# NAVIGATION_CONTAINER
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# ROS
import smach
# TU/e Robotics
import robot_smach_states as states
import robot_smach_states.util.designators as ds
from hmi import HMIResult
from robocup_knowledge import load_knowledge
from robot_skills.util.kdl_conversions import FrameStamped
# Challenge where is this
from .inform_machine import InformMachine
# Load and extract knowledge here so that stuff fails on startup if not defined
knowledge = load_knowledge("challenge_where_is_this")
INFORMATION_POINT_ID = knowledge.information_point_id
INITIAL_POSE_ID = knowledge.initial_pose_id
START_GRAMMAR = knowledge.starting_point_grammar
GRAMMAR = knowledge.location_grammar
START_ROBUST = True # Set this flag to False if you don"t want to use StartChallengeRobust
class WhereIsThis(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
#!/usr/bin/env python
import rospy
import smach
from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator, analyse_designators
import robot_smach_states as states
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_rips')
STARTING_POINT = challenge_knowledge.starting_point
INTERMEDIATE_1 = challenge_knowledge.intermediate_1
INTERMEDIATE_2 = challenge_knowledge.intermediate_2
INTERMEDIATE_3 = challenge_knowledge.intermediate_3
EXIT_1 = challenge_knowledge.exit_1
EXIT_2 = challenge_knowledge.exit_2
EXIT_3 = challenge_knowledge.exit_3
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
# Date: October 2015
################################################
import rospy
import smach
import robot_smach_states as states
import robot_smach_states.util.designators as ds
from robot_smach_states.util.startup import startup
import geometry_msgs.msg as gm
import robot_skills.util.msg_constructors as msgs
from robocup_knowledge import load_knowledge
############################## INITIALIZATIONS ##############################
# load knowledge
common_knowledge = load_knowledge("common")
# challenge_knowledge = load_knowledge("challenge_test")
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints(
"[Challenge Test] ")
############################## MAIN STATE MACHINE ##############################
class ChallengeTemplate(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
with self:
# ROS
import smach
import rospy
# TU/e
import robot_smach_states as states
import robot_smach_states.util.designators as ds
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import load_knowledge
CHALLENGE_KNOWLEDGE = load_knowledge('challenge_take_out_the_garbage')
class DropTrash(smach.State):
"""
State that moves the robot to the drop bag position and opens the gripper
"""
def __init__(self, robot, arm_designator):
"""
:param robot: robot object
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self._robot = robot
self._arm_designator = arm_designator
def execute(self, userdata=None):
arm = self._arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
#!/usr/bin/python
import rospy
import smach
import math
import numpy as np
from robot_smach_states.util.startup import startup
from geometry_msgs.msg import PointStamped
import robot_smach_states as states
from robot_skills.util import transformations, msg_constructors
from robot_skills.util.kdl_conversions import FrameStamped, VectorStamped
from robocup_knowledge import load_knowledge
knowledge = load_knowledge("challenge_restaurant")
from visualize import visualize_location
def _get_area(convex_hull):
pts = [[c.x, c.y] for c in convex_hull]
lines = np.hstack([pts, np.roll(pts, -1, axis=0)])
area = 0.5 * abs(sum(x1 * y2 - x2 * y1 for x1, y1, x2, y2 in lines))
return area
class WaitSay(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=["done"])
self._robot = robot
import robot_skills.util.msg_constructors as msgs
from ed_msgs.msg import EntityInfo
from smach_ros import SimpleActionState
from collections import namedtuple
from dragonfly_speech_recognition.srv import GetSpeechResponse
from robot_smach_states.util.designators import *
from robot_smach_states.human_interaction.human_interaction import HearOptionsExtra
from robot_smach_states import Grab
from robocup_knowledge import load_knowledge
from robot_skills.util import transformations
from robot_skills.arms import Arm
# ----------------------------------------------------------------------------------------------------
common_knowledge = load_knowledge("common")
challenge_knowledge = load_knowledge("challenge_person_recognition")
OBJECT_TYPES = challenge_knowledge.object_types
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints(
"[Challenge Test] ")
# ----------------------------------------------------------------------------------------------------
# OBSELETE! BUT MIGHT BE USEFULL
# class PointAtOperator(smach.State):
# def __init__(self, robot):
# smach.State.__init__(self, outcomes=['done'])
# self.robot = robot
# TU/e
from robot_skills.util.kdl_conversions import FrameStamped
# RoboCup knowledge
from robocup_knowledge import load_knowledge
challenge_knowledge = load_knowledge('challenge_storing_groceries')
# Location
ENTITY_POSES = challenge_knowledge.entity_poses
# Inspection
CABINET = challenge_knowledge.cabinet_amcl
OBJECT_SHELVES = challenge_knowledge.object_shelves
OBJECT_TYPES = challenge_knowledge.object_types
DETECTED_OBJECTS_WITH_PROBS = [
] # List with entities and types. This is used to write to PDF. # ToDo: no global?!
SEGMENTED_ENTITIES = [
] # List with segmented entities such that we can also grasp unknown entities
CLASSIFICATION_THRESHOLD = 0.3 # Threshold for perception. If classification below threshold, the type is not added
# to the world model and further on considered unknown.
MAX_KNOWN_OBJECTS = 10 # Maximum number of known objects to store in the PDF
MAX_UNKNOWN_OBJECTS = 5 # Maximum number of unknown objects to store in the PDF
MIN_OBJECT_HEIGHT = 0.1
# List to skip: these won't be written to the pdf
SKIP_LIST = ["fork", "spoon", "chopsticks"]
# Grasping
TABLE = challenge_knowledge.grasp_surface
# TABLE_POSE = FrameStamped(frame=challenge_knowledge.table_pose, frame_id="map")
import robot_skills.util.msg_constructors as msgs
from robot_skills.util.kdl_conversions import VectorStamped
import time
import smach
import robot_smach_states as states
import robot_smach_states.util.designators as ds
import rospy
from robocup_knowledge import load_knowledge
import math
import random
challenge_knowledge = load_knowledge("challenge_speech_recognition")
class RecognizePersons(smach.State):
def __init__(self, robot):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
def _shot_valid(self, number_of_people, operator_list, detections,
operator):
if not operator:
return False
if len(detections) < number_of_people:
return False
# Check if operator is consistent with operator list
return True
import roslib;
import rospy
import sys
import smach
import smach_ros
import robot_smach_states as states
import robot_smach_states.util.designators as ds
from robot_smach_states.util.startup import startup
import geometry_msgs.msg as gm
import robot_skills.util.msg_constructors as msgs
from robocup_knowledge import load_knowledge
############################## INITIALIZATIONS ##############################
# load knowledge
common_knowledge = load_knowledge("common")
# challenge_knowledge = load_knowledge("challenge_test")
# define print shortcuts from common knowledge
printOk, printError, printWarning = common_knowledge.make_prints("[Challenge Test] ")
############################## MAIN STATE MACHINE ##############################
class ChallengeTest(smach.StateMachine):
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
with self:
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
