Work on spot.

lrs_exec/tst_command.py

@@ -113,6 +113,35 @@ class TstCommand(ExecTree):
         self.timer = self.create_timer(0.1, self.do_command, callback_group=self.group)
 
 
+    def pick_up(self, new=True):
+        try:
+            pick_up = JsonExecNode(self.unit, name="pick-up")
+            tree = JsonExecNode(self.unit, name="seq", node_uuid=str(uuid.uuid4()))            
+            tree.add_child(pick_up)
+            
+            if new:
+                resp = self.new_handle_tree(tree.get_json())
+                print("RESP:", resp)
+            else:
+                (res, reason) = self.handle_tree(tree.get_json(), expand=self.expand)
+                print(res, reason)
+        except Exception as exc:
+            self.get_logger().error(f'EXCEPTION pick_up: {type(exc)} - {exc}')            
+
+    def find_person(self, new=True):
+        try:
+            find_person = JsonExecNode(self.unit, name="find-person")
+            tree = JsonExecNode(self.unit, name="seq", node_uuid=str(uuid.uuid4()))            
+            tree.add_child(find_person)
+            
+            if new:
+                resp = self.new_handle_tree(tree.get_json())
+                print("RESP:", resp)
+            else:
+                (res, reason) = self.handle_tree(tree.get_json(), expand=self.expand)
+                print(res, reason)
+        except Exception as exc:
+            self.get_logger().error(f'EXCEPTION find_person: {type(exc)} - {exc}')            
 
     def fly(self, new=True):
         print("FLY!")
@@ -1139,6 +1168,10 @@ class TstCommand(ExecTree):
             self.timer.cancel()
             if self.command == "look-at-bearing":
                 self.look_at_bearing(self.yaw, self.pitch)
+            if self.command == "pick-up":
+                self.pick_up()
+            if self.command == "find-person":
+                self.find_person()
             if self.command == "fly":
                 self.fly()
             if self.command == "climb":