From 31b8db2d4a2eaaa18144044ed97d165957b3d85e Mon Sep 17 00:00:00 2001
From: Tommy Persson <tommy.persson@liu.se>
Date: Wed, 27 Sep 2023 15:24:41 +0200
Subject: [PATCH] Work on spot.
---
lrs_exec/tst_command.py | 33 +++++++++++++++++++++++++++++++++
1 file changed, 33 insertions(+)
diff --git a/lrs_exec/tst_command.py b/lrs_exec/tst_command.py
index e5c5eff..d71863a 100644
--- a/lrs_exec/tst_command.py
+++ b/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":
--
GitLab