diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml index 5ea1c1c3d..8d28c5b52 100644 --- a/skills/config/motions.yaml +++ b/skills/config/motions.yaml @@ -13,10 +13,10 @@ play_motion: pre_navigation: joints: [torso_lift_joint] points: - - positions: [0.35] + - positions: [0.15] time_from_start: 0.0 post_navigation: - joints: [torso_lift_joint] + joints: [torso_lift_joint, head_1_joint, head_2_joint] points: - - positions: [0.35] + - positions: [0.25, 0.0, 0.0] time_from_start: 0.0 \ No newline at end of file diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py index 7875e7eca..233a436cc 100755 --- a/skills/src/lasr_skills/__init__.py +++ b/skills/src/lasr_skills/__init__.py @@ -5,12 +5,12 @@ from .wait_for_person_in_area import WaitForPersonInArea from .describe_people import DescribePeople from .look_to_point import LookToPoint +from .play_motion import PlayMotion from .go_to_location import GoToLocation from .go_to_semantic_location import GoToSemanticLocation from .say import Say from .listen import Listen from .listen_for import ListenFor -from .play_motion import PlayMotion from .receive_object import ReceiveObject from .handover_object import HandoverObject from .ask_and_listen import AskAndListen diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index ef4bd59a5..e1142f0a4 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -28,21 +28,23 @@ class GoToLocation(smach.StateMachine): def __init__(self, location: Union[Pose, None] = None): super(GoToLocation, self).__init__(outcomes=["succeeded", "failed"]) + IS_SIMULATION = ( + "/pal_startup_control/start" not in rosservice.get_service_list() + ) + with self: smach.StateMachine.add( "LOWER_BASE", PlayMotion("pre_navigation"), transitions={ - "succeeded": "ENABLE_HEAD_MANAGER", + "succeeded": ( + "ENABLE_HEAD_MANAGER" if not IS_SIMULATION else "GO_TO_LOCATION" + ), "aborted": "failed", "preempted": "failed", }, ) - IS_SIMULATION = ( - "/pal_startup_control/start" in rosservice.get_service_list() - ) - if not IS_SIMULATION: if PUBLIC_CONTAINER: @@ -51,11 +53,17 @@ def __init__(self, location: Union[Pose, None] = None): ) else: smach.StateMachine.add( - smach_ros.SimpleServiceState( + "ENABLE_HEAD_MANAGER", + smach_ros.ServiceState( "/pal_startup_control/start", StartupStart, request=StartupStartRequest("head_manager", ""), ), + transitions={ + "succeeded": "GO_TO_LOCATION", + "preempted": "failed", + "aborted": "failed", + }, ) if location is not None: @@ -71,7 +79,11 @@ def __init__(self, location: Union[Pose, None] = None): ), ), transitions={ - "succeeded": "DISABLE_HEAD_MANAGER", + "succeeded": ( + "DISABLE_HEAD_MANAGER" + if not IS_SIMULATION + else "RAISE_BASE" + ), "aborted": "failed", "preempted": "failed", }, @@ -90,7 +102,11 @@ def __init__(self, location: Union[Pose, None] = None): input_keys=["location"], ), transitions={ - "succeeded": "DISABLE_HEAD_MANAGER", + "succeeded": ( + "DISABLE_HEAD_MANAGER" + if not IS_SIMULATION + else "RAISE_BASE" + ), "aborted": "failed", "preempted": "failed", }, @@ -105,14 +121,26 @@ def __init__(self, location: Union[Pose, None] = None): else: smach.StateMachine.add( "DISABLE_HEAD_MANAGER", - smach_ros.SimpleServiceState( + smach_ros.ServiceState( "/pal_startup_control/stop", StartupStop, request=StartupStopRequest("head_manager"), ), transitions={ - "succeeded": "succeeded", - "aborted": "succeeded", - "preempted": "succeeded", + "succeeded": "RAISE_BASE", + "aborted": "failed", + "preempted": "failed", }, ) + + smach.StateMachine.add( + "RAISE_BASE", + PlayMotion("post_navigation"), + transitions={ + "succeeded": ( + "ENABLE_HEAD_MANAGER" if not IS_SIMULATION else "GO_TO_LOCATION" + ), + "aborted": "failed", + "preempted": "failed", + }, + ) diff --git a/skills/src/lasr_skills/play_motion.py b/skills/src/lasr_skills/play_motion.py index 525e84117..b2b81a372 100644 --- a/skills/src/lasr_skills/play_motion.py +++ b/skills/src/lasr_skills/play_motion.py @@ -1,4 +1,5 @@ import smach_ros +import rospy from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal @@ -11,14 +12,24 @@ def __init__(self, motion_name: Union[str, None] = None): super(PlayMotion, self).__init__( "play_motion", PlayMotionAction, - goal=PlayMotionGoal(motion_name=motion_name, skip_planning=False), + goal=PlayMotionGoal( + motion_name=motion_name, + skip_planning=rospy.get_param( + f"/play_motion/motions/{motion_name}/joints" + ) + == ["torso_lift_joint"], + ), ) else: super(PlayMotion, self).__init__( "play_motion", PlayMotionAction, goal_cb=lambda ud, _: PlayMotionGoal( - motion_name=ud.motion_name, skip_planning=False + motion_name=ud.motion_name, + skip_planning=rospy.get_param( + f"/play_motion/motions/{ud.motion_name}/joints" + == ["torso_lift_joint"] + ), ), input_keys=["motion_name"], )