diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml index eaedd231b..8d28c5b52 100644 --- a/skills/config/motions.yaml +++ b/skills/config/motions.yaml @@ -9,4 +9,14 @@ play_motion: joints: [gripper_left_finger_joint, gripper_right_finger_joint] points: - positions: [0.04, 0.04] + time_from_start: 0.0 + pre_navigation: + joints: [torso_lift_joint] + points: + - positions: [0.15] + time_from_start: 0.0 + post_navigation: + joints: [torso_lift_joint, head_1_joint, head_2_joint] + points: + - 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 5efeae034..e1142f0a4 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -1,18 +1,146 @@ import smach_ros - +import smach +import rospy +import rosservice from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Pose, PoseStamped from std_msgs.msg import Header +from typing import Union + +from lasr_skills import PlayMotion + +PUBLIC_CONTAINER = False + +try: + from pal_startup_msgs.srv import ( + StartupStart, + StartupStop, + StartupStartRequest, + StartupStopRequest, + ) +except ModuleNotFoundError: + PUBLIC_CONTAINER = True -class GoToLocation(smach_ros.SimpleActionState): - def __init__(self): - super(GoToLocation, self).__init__( - "move_base", - MoveBaseAction, - goal_cb=lambda ud, _: MoveBaseGoal( - target_pose=PoseStamped(pose=ud.location, header=Header(frame_id="map")) - ), - input_keys=["location"], +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" if not IS_SIMULATION else "GO_TO_LOCATION" + ), + "aborted": "failed", + "preempted": "failed", + }, + ) + + if not IS_SIMULATION: + + if PUBLIC_CONTAINER: + rospy.logwarn( + "You are using a public container. The head manager will not be stopped during navigation." + ) + else: + smach.StateMachine.add( + "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: + smach.StateMachine.add( + "GO_TO_LOCATION", + smach_ros.SimpleActionState( + "move_base", + MoveBaseAction, + goal=MoveBaseGoal( + target_pose=PoseStamped( + pose=location, header=Header(frame_id="map") + ) + ), + ), + transitions={ + "succeeded": ( + "DISABLE_HEAD_MANAGER" + if not IS_SIMULATION + else "RAISE_BASE" + ), + "aborted": "failed", + "preempted": "failed", + }, + ) + else: + smach.StateMachine.add( + "GO_TO_LOCATION", + smach_ros.SimpleActionState( + "move_base", + MoveBaseAction, + goal_cb=lambda ud, _: MoveBaseGoal( + target_pose=PoseStamped( + pose=ud.location, header=Header(frame_id="map") + ) + ), + input_keys=["location"], + ), + transitions={ + "succeeded": ( + "DISABLE_HEAD_MANAGER" + if not IS_SIMULATION + else "RAISE_BASE" + ), + "aborted": "failed", + "preempted": "failed", + }, + ) + + if not IS_SIMULATION: + + if PUBLIC_CONTAINER: + rospy.logwarn( + "You are using a public container. The head manager will not be start following navigation." + ) + else: + smach.StateMachine.add( + "DISABLE_HEAD_MANAGER", + smach_ros.ServiceState( + "/pal_startup_control/stop", + StartupStop, + request=StartupStopRequest("head_manager"), + ), + transitions={ + "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"], )