feat: add mapping srv state and resent

LASR-at-Home · Jul 17, 2024 · 70b0c86 · 70b0c86
1 parent 4ab450c
commit 70b0c86
Showing 1 changed file with 15 additions and 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
@@ -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",