From 9431c02abc3f641fe3a18c3348c8c1f192634f6e Mon Sep 17 00:00:00 2001
From: Tommy Persson <tommy.persson@liu.se>
Date: Tue, 19 Sep 2023 18:13:22 +0200
Subject: [PATCH] Fix of search areas
---
lrs_exec/scenario_handler.py | 67 ++++++++++++++++++++++++++++++++++--
tmux/op.tmux | 8 ++---
2 files changed, 68 insertions(+), 7 deletions(-)
diff --git a/lrs_exec/scenario_handler.py b/lrs_exec/scenario_handler.py
index 5537de2..2cdf2ee 100644
--- a/lrs_exec/scenario_handler.py
+++ b/lrs_exec/scenario_handler.py
@@ -240,10 +240,71 @@ class ScenarioHandler(Node):
for i in range(len(msg.package_types)):
self.n_packages_in_bucket[msg.package_types[i]] = msg.number_of_packages_in_body_bucket[i]
+
+
+# "name": "search-area",
+# "params": {
+# "area": [
+# {
+# "altitude": 58,
+# "latitude": 57.760412547494035,
+# "longitude": 16.68256568978499,
+# "rostype": "GeoPoint"
+# },
+# {
+# "altitude": 58,
+# "latitude": 57.760401097175794,
+# "longitude": 16.682585502904335,
+# "rostype": "GeoPoint"
+# },
+# {
+# "altitude": 58,
+# "latitude": 57.76068030775089,
+# "longitude": 16.68314852572718,
+# "rostype": "GeoPoint"
+# },
+# {
+# "altitude": 58,
+# "latitude": 57.76068465130913,
+# "longitude": 16.683144271373752,
+# "rostype": "GeoPoint"
+# }
+# ],
+# "speed": "standard",
+#
+
+
+
def add_search_to_queue(self):
- path = [[40, 0, 20], [40, 80, 20], [0, 0, 20]]
- print("AREA:", path)
- area = self.world_to_geo_path(path)
+ #path = [[-5, -5, 0], [15, 15, 0], [20, -10, 0], [5, -10, 0]]
+ #print("AREA:", path)
+ #area = self.world_to_geo_path(path)
+ area = [
+ {
+ "altitude": 58,
+ "latitude": 57.760412547494035,
+ "longitude": 16.68256568978499,
+ "rostype": "GeoPoint"
+ },
+ {
+ "altitude": 58,
+ "latitude": 57.760401097175794,
+ "longitude": 16.682585502904335,
+ "rostype": "GeoPoint"
+ },
+ {
+ "altitude": 58,
+ "latitude": 57.76068030775089,
+ "longitude": 16.68314852572718,
+ "rostype": "GeoPoint"
+ },
+ {
+ "altitude": 58,
+ "latitude": 57.76068465130913,
+ "longitude": 16.683144271373752,
+ "rostype": "GeoPoint"
+ }
+ ]
print("AREA:", area)
sa = JsonExecNode("", name="basic-search-area", area=area, speed="slow", target_type="person", target_size=2, area_type="field")
tree = JsonExecNode("", name="seq", node_uuid=str(uuid.uuid4()))
diff --git a/tmux/op.tmux b/tmux/op.tmux
index bdf6cbc..313d27f 100755
--- a/tmux/op.tmux
+++ b/tmux/op.tmux
@@ -99,11 +99,11 @@ def main_unit(ns):
ros2("teams", f"ros2 launch lrs_team team.launch.py ns:={ns} {mqtt_host} mqtt_port:={options.wara_broker_port} {mqtt_user} {wara_password}")
if options.spot:
ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=deliver -p team_command:=set-team -p args:=\\\"spot0\\\"')
- ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=search -p team_command:=set-team -p args:=\\\"dji5\\\"')
+ ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=search -p team_command:=set-team -p args:=\\\"dji8\\\"')
ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=drop -p team_command:=set-team -p args:=\\\"dji17\\\"')
else:
ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchsurface -p team_command:=set-team -p args:=\\\"Piraya14 Manta\\\"')
- ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchair -p team_command:=set-team -p args:=\\\"dji8\\\"')
+ ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=searchair -p team_command:=set-team -p args:=\\\"dji5\\\"')
ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=examine -p team_command:=set-team -p args:=\\\"MiniUSV1 MiniUSV2 MiniUSV3 MiniUSV4\\\"')
ros2("create_team", f'ros2 run lrs_team team_command --ros-args -r __ns:={ns} -p team_name:=sup -p team_command:=set-team -p args:=\\\"expand\\\"')
@@ -165,10 +165,10 @@ def main_unit(ns):
ros2("tst_command", f'ros2 run lrs_exec tst_command --ros-args -r __ns:={ns} -p command:="debug-1" -p name:="test" -p z:=20.0 -p x:=10.0 -p y:=10.0', run=False)
if options.spot:
- ros2("packages_server", "ros2 run lrs_wdb packages --ros-args -r __ns:=/op0")
- ros2("scenario_handler", "ros2 run lrs_exec scenario_handler --ros-args -r __ns:=/op0 -p fake:=true")
ros2("team_ui", "rqt -s lrs_rqt_team.teamqueuesplugin.TeamQueues --ros-args -r __ns:=/op0", run=False)
ros2("spot_status", "ros2 topic echo /op0/global/spot_status")
+ ros2("packages_server", "ros2 run lrs_wdb packages --ros-args -r __ns:=/op0")
+ ros2("scenario_handler", "ros2 run lrs_exec scenario_handler --ros-args -r __ns:=/op0 -p fake:=true -p drop:=true -p search:=true", run=False)
ros2("start_spot_scenario", "ros2 run lrs_mqtt test_salient --ros-args -p mqtt_host:=10.8.0.77 -p demo:=true -p person:=0", run=False)
--
GitLab