Fix of search areas

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()))

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)