feat: late night fixes

Author: fireblonde

common/navigation/lasr_person_following/nodes/person_following.py

@@ -85,8 +85,8 @@ def _update_params(self):
         self._dynamic_recovery(config)
 
         config = Config()
-        config.bools.append(BoolParameter(name="enabled"), value=1)
-        config.doubles.append(DoubleParameter(name="inflation_radius"), value=0.2)
+        config.bools.append(BoolParameter(name="enabled", value=1))
+        config.doubles.append(DoubleParameter(name="inflation_radius", value=0.2))
 
         self._dynamic_local_costmap(config)
 

common/navigation/lasr_person_following/src/lasr_person_following/person_following.py

@@ -342,15 +342,15 @@ def _cancel_goal(self) -> None:
 
     def _check_finished(self) -> bool:
         if self._tts_client_available:
-            self._tts("Have we arrived?", wait=True)
+            self._tts("Have we arrived? Please say Yes We Have Arrived", wait=True)
 
             if self._transcribe_speech_client_available:
                 self._transcribe_speech_client.send_goal_and_wait(
                     TranscribeSpeechGoal()
                 )
                 transcription = self._transcribe_speech_client.get_result().sequence
 
-                return "yes" in transcription.lower()
+                return "yes" in transcription.lower() or "arrived" in transcription.lower() or "arrive" in transcription.lower()
         return True
 
     def _get_pose_on_path(
@@ -416,11 +416,6 @@ def follow(self) -> FollowResult:
 
         while not rospy.is_shutdown():
 
-            if self._should_stop:
-                rospy.loginfo("Received stop signal")
-                self._tts("I am stopping now", wait=True)
-                break
-
             tracks: PersonArray = rospy.wait_for_message("/people_tracked", PersonArray)
 
             # Get the most recent track of the person we are following

skills/src/lasr_skills/receive_object.py

@@ -147,7 +147,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True):
                 smach.StateMachine.add(
                     "SAY_PLACE",
                     Say(
-                        text=f"Please place the {object_name} in my end-effector. I will wait for a few seconds.",
+                        text=f"Please place the {object_name} in my hand. I will wait for a few seconds.",
                     ),
                     transitions={
                         "succeeded": "WAIT_5",
@@ -159,7 +159,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True):
                 smach.StateMachine.add(
                     "SAY_PLACE",
                     Say(
-                        format_str="Please place the {} in my end-effector. I will wait for a few seconds.",
+                        format_str="Please place the {} in my hand. I will wait for a few seconds.",
                     ),
                     transitions={
                         "succeeded": "WAIT_5",

tasks/carry_my_luggage/src/carry_my_luggage/state_machine.py

@@ -177,66 +177,53 @@ def wait_cb(ud, msg):
                 },
             )
 
+            # TODO: ensure it doesnt get stuck here, maybe concurrenr
             smach.StateMachine.add(
                 "CLEAR_COSTMAPS",
                 smach_ros.ServiceState(
                     "/move_base/clear_costmaps",
                     EmptySrv,
                 ),
                 transitions={
-                    "succeeded": "START_MAPPING",
-                    "aborted": "START_MAPPING",
-                    "preempted": "START_MAPPING",
+                    "succeeded": "GET_START_POSE",
+                    "aborted": "GET_START_POSE",
+                    "preempted": "GET_START_POSE",
                 },
             )
 
+            def start_pose_cb(ud):
+                try:
+                    ud.start_pose = rospy.wait_for_message("/robot_pose", PoseWithCovarianceStamped, timeout=rospy.Duration(5.0)).pose.pose
+                except rospy.ROSException:
+                    rospy.logerr("Failed to get robot pose")
+                    ud.start_pose = Pose(position=Point(0., 0., 0.0), orientation=Quaternion(0., 0., 0., 1.))
+                return "succeeded"
             smach.StateMachine.add(
-                "START_MAPPING",
-                smach_ros.ServiceState(
-                    "/pal_navigation_sm",
-                    Acknowledgment,
-                    request=AcknowledgmentRequest("MAP"),
+                "GET_START_LOCATION",
+                smach.CBState(
+                    start_pose_cb,
+                    outcomes=["succeeded"],
+                    output_keys=["start_pose"],
                 ),
-                transitions={
-                    "succeeded": "SAY_FOLLOW",
-                    "aborted": "SAY_FOLLOW",
-                    "preempted": "SAY_FOLLOW",
-                },
+                transitions={"succeeded": "SAY_STEP"}
             )
 
-            smach.StateMachine.add(
-                "SAY_FOLLOW",
-                Say(text="I will follow you now."),
-                transitions={
-                    "succeeded": "SAY_STEP",
-                    # "succeeded": "INITIALISE_PERSON_TRACK",
-                    "aborted": "failed",
-                    "preempted": "failed",
-                },
-            )
             smach.StateMachine.add(
                 "SAY_STEP",
                 Say(text="First walk slowly towards me and then I will follow you."),
                 transitions={
-                    "succeeded": "FOLLOW",
+                    "succeeded": "SAY_FOLLOW",
                     "aborted": "failed",
                     "preempted": "failed",
                 },
             )
             smach.StateMachine.add(
-                "INITIALISE_PERSON_TRACK",
-                smach_ros.ServiceState(
-                    "/cml/initialise_person_with_vision",
-                    InitialisePersonWithVision,
-                    request_cb=lambda ud, req: InitialisePersonWithVisionRequest(
-                        point=ud.person_point,
-                    ),
-                    input_keys=["person_point"],
-                ),
+                "SAY_FOLLOW",
+                Say(text="I will follow you now."),
                 transitions={
                     "succeeded": "FOLLOW",
-                    "aborted": "FOLLOW",
-                    "preempted": "FOLLOW",
+                    "aborted": "failed",
+                    "preempted": "failed",
                 },
             )
 
@@ -283,12 +270,8 @@ def wait_cb(ud, msg):
             )
 
             smach.StateMachine.add(
-                "GO_TO_START",
-                GoToLocation(
-                    location=Pose(
-                        position=Point(0.0, 0.0, 0.0),
-                        orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
-                    )
-                ),
+                "GO_TO_START", # todo: instead, get the start position within the state machine, and return to it at the end
+                GoToLocation(),
+                remapping={"location" : "start_pose"},
                 transitions={"succeeded": "succeeded", "failed": "succeeded"},
             )