From 70b0c86b5c036c31abce5b93ec1a8f4ac1666483 Mon Sep 17 00:00:00 2001 From: fireblonde Date: Wed, 17 Jul 2024 17:46:10 +0100 Subject: [PATCH] feat: add mapping srv state and resent --- .../src/carry_my_luggage/state_machine.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py b/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py index 0f8ae49b6..9c5b8a804 100644 --- a/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py +++ b/tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py @@ -7,7 +7,6 @@ DetectGesture, ReceiveObject, HandoverObject, -) from lasr_skills.vision import GetCroppedImage from lasr_skills import PlayMotion from lasr_person_following.msg import FollowAction @@ -17,6 +16,7 @@ ) from std_msgs.msg import Empty +from pal_navigation_msgs.srv import Acknowledgment, AcknowledgmentRequest from std_srvs.srv import Empty as EmptySrv from pal_startup_msgs.srv import ( @@ -49,7 +49,6 @@ def execute(self, userdata): def __init__(self): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: - def wait_cb(ud, msg): rospy.loginfo("Received start signal") return False @@ -179,6 +178,20 @@ def wait_cb(ud, msg): "/move_base/clear_costmaps", EmptySrv, ), + transitions={ + "succeeded": "START_MAPPING", + "aborted": "START_MAPPING", + "preempted": "START_MAPPING", + }, + ) + + smach.StateMachine.add( + "START_MAPPING", + smach_ros.ServiceState( + "/pal_navigation_sm", + Acknowledgment, + request=AcknowledgmentRequest("MAP"), + ), transitions={ "succeeded": "SAY_FOLLOW", "aborted": "SAY_FOLLOW",