Merge pull request #932 from tue-robotics/feature/examples_smach_states

Feature/examples smach states

robot_smach_states/examples/example_segment_objects.py

@@ -0,0 +1,41 @@
+# ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states.util import designators as ds
+from robot_smach_states import SegmentObjects
+
+from robot_skills.classification_result import ClassificationResult
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Test if it can segment objects")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    parser.add_argument("entity", default="dinner_table", help="Name of the entity on which to segment objects")
+    args = parser.parse_args()
+
+    rospy.init_node("example_segment_objects")
+
+    robot = get_robot(args.robot)
+    location_name = args.entity
+
+    rospy.loginfo("Creating location designator")
+    location_designator = ds.EntityByIdDesignator(robot, location_name)
+
+    rospy.loginfo("Creating found entity designator")
+    segmented_entities_designator = ds.VariableDesignator([], resolve_type=[ClassificationResult])
+
+    rospy.loginfo("Creating segment objects state")
+    sm = SegmentObjects(robot, segmented_entities_designator.writeable, location_designator, "on_top_of")
+
+    rospy.loginfo("Executing segment objects state")
+    sm.execute()
+
+    rospy.loginfo("Resulting classification result: \n {}".format(segmented_entities_designator.resolve()))
+
+    rospy.loginfo("Done")

robot_smach_states/examples/human_interaction/example_detect_faces.py

@@ -0,0 +1,22 @@
+#!/usr/bin/python
+
+# ROS
+import rospy
+import argparse
+
+# Detect face
+from robot_skills import get_robot
+from robot_smach_states import DetectFace
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Test if the robot can detect the face of the person standing in front"
+                                                 "of it")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node('test_detect_faces')
+    robot = get_robot(args.robot)
+
+    detect_state = DetectFace(robot=robot)
+    detect_state.execute(None)

robot_smach_states/examples/human_interaction/example_give_directions.py

@@ -0,0 +1,26 @@
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_smach_states.human_interaction import give_directions
+import robot_smach_states.util.designators as ds
+from robot_skills.get_robot import get_robot
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Starting from the current robot position and orientation give "
+                                                 "directions to the desired target object")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    parser.add_argument("target_entity_id", type=str, help="Entity id of the target object")
+    args = parser.parse_args()
+
+    # Create node
+    rospy.init_node("test_give_directions")
+    robot = get_robot(args.robot)
+    e_id = args.target_entity_id
+
+    # Instantiate GuideToSymbolic machine
+    state = give_directions.GiveDirections(r, ds.EntityByIdDesignator(r, id=e_id))
+    # Execute the state
+    state.execute()

robot_smach_states/examples/human_interaction/example_hear_options.py

@@ -0,0 +1,21 @@
+# ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states.human_interaction import HearOptions
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Test if the robot hears the things from the options available")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node('test_hear_options')
+    robot = get_robot(args.robot)
+
+    hear_option_state = HearOptions(robot, ['no', 'yes'])
+    hear_option_state.execute()

robot_smach_states/examples/human_interaction/example_say.py

@@ -0,0 +1,22 @@
+#ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states.human_interaction import Say
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Test the say state with a sentence")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node('test_say')
+    robot = get_robot(args.robot)
+    sentence = 'I have said something useful'
+
+    say_state = Say(robot, sentence)
+    say_state.execute()

robot_smach_states/examples/human_interaction/example_show_image.py

@@ -0,0 +1,30 @@
+import argparse
+
+#ROS
+import rospy
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states.human_interaction import ShowImageState
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Show an image on the robot display.")
+    parser.add_argument(
+        "file_path",
+        type=str,
+        help="String describing the path to the image (absolute path incl. file extension)"
+    )
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node('example_show_image')
+
+    robot = get_robot(args.robot)
+    file_path = args.file_path
+
+    show_image_state = ShowImageState(robot, file_path)
+    show_image_state.execute()

robot_smach_states/examples/human_interaction/example_wait_for_person_in_front.py

@@ -0,0 +1,31 @@
+# ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states.human_interaction import WaitForPersonInFront
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Test to see if the robot detects a person in front of him")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("example_wait_for_person_in_front")
+
+    robot = get_robot(args.robot)
+
+    rospy.loginfo("Creating wait for person in front state")
+    attempts = 10
+    sleep_interval = 0.2
+    sm = WaitForPersonInFront(robot, attempts, sleep_interval)
+    outcome = sm.execute()
+
+    rospy.loginfo("Executing wait for person in front state")
+    if outcome == "success":
+        rospy.loginfo("Detected person")
+    else:
+        rospy.loginfo("No person detected")

robot_smach_states/examples/example_handover_to_human.py → robot_smach_states/examples/manipulation/example_handover_to_human.py

@@ -1,9 +1,9 @@
 # ROS
 import rospy
-import sys
+import argparse
 
 # TU/e Robotics
-from robot_skills.get_robot import get_robot_from_argv
+from robot_skills.get_robot import get_robot
 
 # Robot Smach States
 import robot_smach_states.util.designators as ds
@@ -12,12 +12,12 @@
 
 
 if __name__ == "__main__":
-    assert len(sys.argv) == 2, "Please provide the robot name" \
-                               "e.g., 'python example_handover_to_human.py hero'"
+    parser = argparse.ArgumentParser(description="Test handover to human state")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
 
     rospy.init_node("test_handover_stuff")
-
-    robot = get_robot_from_argv(index=1)
+    robot = get_robot(args.robot)
 
     rospy.loginfo("Creating arm designator")
     arm_designator = ds.UnoccupiedArmDesignator(robot=robot,

robot_smach_states/examples/manipulation/example_set_gripper.py

@@ -0,0 +1,28 @@
+#ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+from robot_skills.arms import GripperState
+
+# Robot Smach States
+from robot_smach_states.manipulation import SetGripper
+import robot_smach_states.util.designators as ds
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="Test the gripper state whether it can open and close")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("Set_gripper_state")
+    robot = get_robot(args.robot)
+    arm = ds.UnoccupiedArmDesignator(robot, {})
+
+    for gripper_state in [GripperState.CLOSE, GripperState.OPEN]:
+        set_gripper_state = SetGripper(robot, arm, gripperstate=gripper_state)
+        outcome = set_gripper_state.execute()
+        rospy.loginfo("Set gripper state to {}, outcome: {}".format(gripper_state, outcome))
+        rospy.sleep(2.5)

robot_smach_states/examples/navigation/example_force_drive.py

@@ -0,0 +1,40 @@
+# ROS
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_skills.get_robot import get_robot
+
+# Robot Smach States
+from robot_smach_states import ForceDrive
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Test the force drive of the robot")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    args = parser.parse_args()
+
+    rospy.init_node("example_force_drive")
+    robot = get_robot(args.robot)
+
+    sentence = "I am going to execute a force drive, press enter if the area around me is clear"
+    robot.speech.speak(sentence)
+    confirm = raw_input(sentence)
+
+    rospy.loginfo("Creating forward force drive state")
+    vx = 0.3
+    t = 1.0
+    sm = ForceDrive(robot, vx, 0, 0, t)
+
+    rospy.loginfo("Executing forward force drive state")
+    sm.execute()
+
+    rospy.loginfo("Creating rotating force drive state")
+    vtheta = 1.57
+    t = 2.0
+    sm = ForceDrive(robot, 0, 0, vtheta, t)
+
+    rospy.loginfo("Executing forward force drive state")
+    sm.execute()
+
+    rospy.loginfo("Done")

robot_smach_states/examples/test_guidance.py → robot_smach_states/examples/navigation/example_guidance.py

@@ -1,10 +1,10 @@
 import os
 import rospy
-import sys
+import argparse
 import std_srvs.srv
 import robot_smach_states.util.designators as ds
 
-from robot_skills.get_robot import get_robot_from_argv
+from robot_skills.get_robot import get_robot
 from robot_smach_states.navigation import guidance
 
 OPERATOR_AVAILABLE = True
@@ -24,13 +24,15 @@ def toggle_operator(_):
 
 if __name__ == "__main__":
 
-    assert len(sys.argv) == 3, "Please provide the robot name and the entity id of the object to guide to," \
-                               "e.g., 'python guidance.py amigo bed'"
+    parser = argparse.ArgumentParser(description="Test the guidance state")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    parser.add_argument("entity", help="Entity name of the object to guide to, for example dinner_table")
+    args = parser.parse_args()
 
     # Create node, robot and toggle interface
     rospy.init_node("test_guidance")
-    r = get_robot_from_argv(1)
-    e_id = sys.argv[2]
+    r = get_robot(args.robot)
+    e_id = args.entity
 
     # Instantiate GuideToSymbolic machine
     s = guidance.GuideToSymbolic(r,

robot_smach_states/examples/navigation/example_tour_guide.py

@@ -0,0 +1,33 @@
+import rospy
+import argparse
+
+# TU/e Robotics
+from robot_smach_states.navigation import guidance
+from robot_skills.get_robot import get_robot
+
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="While the robot is driving to a nav goal, this state tells the rooms"
+                                                 "and furniture objects the robot encounters")
+    parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
+    parser.add_argument("--x_threshold", type=float, default=0.75, help="threshold determining when a piece of "
+                                                                        "furniture is close enough to be described [m]")
+    parser.add_argument("--y_threshold", type=float, default=1.5, help="threshold determining when a piece of furniture"
+                                                                       " is close enough to be described [m]")
+    args = parser.parse_args()
+
+    # Create node, robot and tour_guide instance
+    rospy.init_node("example_tour_guide")
+    robot = get_robot(args.robot)
+    state = guidance.TourGuide(robot, args.x_threshold, args.y_threshold)
+
+    # Initialize
+    state.initialize()
+
+    rate = rospy.Rate(1)
+    # Keep explaining the robot movement
+    while not rospy.is_shutdown():
+        rospy.loginfo(state.describe_near_objects())
+        rate.sleep()
+