Planner frequency test

src/simulators/morse_ros/morse_ros/configs/data_gui.py

@@ -0,0 +1,61 @@
+# -*- coding: utf-8 -*-
+#!/usr/bin/env python
+# Brief: GUI for collecting map data
+# Author: Phani Teja Singamaneni
+
+import rospy
+from geometry_msgs.msg import PointStamped
+import PySimpleGUI as sg
+import yaml
+
+class GetPos(object):
+  """docstring for GetPos."""
+  def __init__(self):
+    super(GetPos, self).__init__()
+    rospy.init_node('GetData')
+    rospy.Subscriber('/clicked_point', PointStamped, self.posCB)
+    self.center = [0, 0]
+    self.triggered = False
+
+  def posCB(self, msg):
+    self.center = [msg.point.x, msg.point.y]
+    self.triggered = True
+
+
+
+sg.theme('DarkAmber')   # Add a touch of color
+# All the stuff inside your window.
+layout = [  [sg.Text('Center ='), sg.Text('0', size=(50,1), key='-mytext-')],
+            [sg.Text('Enter the radius'), sg.InputText()],
+            [sg.Button('Add'), sg.Button('Discard'), sg.Button('Close')] ]
+
+# Create the Window
+window = sg.Window('Get Data', layout)
+# Event Loop to process "events" and get the "values" of the inputs
+
+get_point = GetPos()
+data = dict()
+data['centers'] = []
+data['radii'] = []
+while True:
+    if get_point.triggered:
+      window['-mytext-'].update('Got the Center!')
+      data['centers'].append(get_point.center)
+      # print(get_point.center)
+      event, values = window.read()
+      if event == 'Add':
+        window['-mytext-'].update('Added the data Point')
+        event, values = window.read(1000)
+        data['radii'].append(float(values[0]))
+      get_point.triggered = False;
+    else:
+      event, values = window.read(100)
+      window['-mytext-'].update('Waiting for the center...')
+
+    if event == sg.WIN_CLOSED or event == 'Close': # if user closes window or clicks cancel
+        break
+
+with open('test.yaml', 'w') as file:
+    documents = yaml.dump(data, file, default_flow_style=False)
+
+window.close()

src/simulators/morse_ros/morse_ros/configs/laas_adream.yaml

@@ -1,34 +1,125 @@
-num_humans: 3 # Number should match the number of individual human configs from below
-
-human_1:
-  loop_path: True
-  planner_name: 'gbl_planner/make_plan'
-  scenarios:
-    selected_id: 2 # ID to select from the goal sequence below. Starts with 0
-    goals: [ # Sequence provided by scenerio
-      [[1.0, 1.0], [2.0, 2.0]],
-      [[2.0, 1.0]],
-      [[8.5, 2.31], [8.5, -5.0]]
-    ]
-
-human_2:
-  loop_path: True
-  planner_name: 'gbl_planner/make_plan'
-  scenarios:
-    selected_id: 2
-    goals: [ # Sequence provided by scenerio
-      [[1.0, 1.0], [2.0, 2.0]],
-      [[2.0, 1.0]],
-      [[8.5, -5.0], [8.5, 2.31]]
-    ]
-
-human_3:
-  loop_path: True
-  planner_name: 'gbl_planner/make_plan'
-  scenarios:
-    selected_id: 2
-    goals: [ # Sequence provided by scenerio
-      [[1.0, 1.0], [2.0, 2.0]],
-      [[2.0, 1.0]],
-      [[10.0, 15.0], [10.0, 2.00]]
-    ]
+centers:
+- - 2.1506237983703613
+  - -4.213078022003174
+- - 0.8605523109436035
+  - -0.9781403541564941
+- - 0.845545768737793
+  - 0.9405002593994141
+- - 0.8774352073669434
+  - 2.967954635620117
+- - 3.938210964202881
+  - -1.586620807647705
+- - 3.5395851135253906
+  - 2.178901195526123
+- - 0.6803431510925293
+  - 4.737829208374023
+- - 0.6803431510925293
+  - 6.157594203948975
+- - 0.6803426742553711
+  - 7.84291934967041
+- - 0.7122383117675781
+  - 9.329312324523926
+- - 0.6954741477966309
+  - 10.765373229980469
+- - 0.7122392654418945
+  - 12.089086532592773
+- - 0.7122373580932617
+  - 13.113795280456543
+- - 2.6300883293151855
+  - 15.441251754760742
+- - 2.2190794944763184
+  - 19.681180953979492
+- - 4.9456281661987305
+  - 16.878816604614258
+- - 6.086401462554932
+  - 14.085185050964355
+- - 6.9962897300720215
+  - 15.86962890625
+- - 7.111672878265381
+  - 17.272050857543945
+- - 5.126888751983643
+  - 19.282974243164062
+- - 8.713769912719727
+  - 14.017230987548828
+- - 9.228250503540039
+  - 16.727148056030273
+- - 7.624399662017822
+  - 19.61571502685547
+- - 10.499322891235352
+  - 19.315454483032227
+- - 9.60455322265625
+  - 21.46706199645996
+- - 6.067229747772217
+  - 21.63995361328125
+- - 10.58148193359375
+  - 12.162261009216309
+- - 10.544995307922363
+  - 9.830551147460938
+- - 10.513105392456055
+  - 6.942296981811523
+- - 10.496223449707031
+  - 4.781736373901367
+- - 10.529989242553711
+  - 2.156038761138916
+- - 10.54499626159668
+  - 0.025473594665527344
+- - 7.667494773864746
+  - 1.168482780456543
+- - 8.493816375732422
+  - -3.6860713958740234
+- - 7.090419769287109
+  - 5.807111740112305
+- - 5.618354320526123
+  - 7.474330425262451
+- - 5.584620475769043
+  - 10.62723159790039
+- - 4.146762371063232
+  - 9.340786933898926
+- - 5.7524871826171875
+  - 9.176801681518555
+- - 7.155835151672363
+  - 7.937747478485107
+- - 8.064230918884277
+  - 10.546623229980469
+radii:
+- 3.56
+- 1.35
+- 1.35
+- 1.35
+- 2.3
+- 3.7
+- 0.7
+- 0.7
+- 0.7
+- 0.7
+- 0.7
+- 0.7
+- 0.7
+- 4.0
+- 1.6
+- 2.7
+- 2.5
+- 1.5
+- 1.5
+- 1.5
+- 2.3
+- 3.0
+- 1.8
+- 1.4
+- 3.0
+- 3.0
+- 1.2
+- 1.2
+- 1.2
+- 1.2
+- 1.5
+- 1.5
+- 3.5
+- 4.4
+- 2.5
+- 1.0
+- 1.0
+- 0.8
+- 0.8
+- 0.8
+- 2.2

src/simulators/morse_ros/morse_ros/configs/laas_adream_init.yaml

@@ -0,0 +1,113 @@
+num_humans: 4 # Number should match the number of individual human configs from below
+
+#scenario 0 normal ORCA humans going around
+#scenario 1 - ORCA, Inhus and CoHAN
+human_1:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 1 # ID to select from the goal sequence below. Starts with 0
+    goals: [ # Sequence provided by scenerio
+      [[4.5, -5.6], [11.0, 22.1]],
+      [[7.75, 5.0], [8.6, 22.9]],
+      [[8.5, 2.31], [8.5, -5.0]]
+    ]
+
+human_2:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 0 # same for 1 as well
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 2.4], [11.0, 19.6]], #[5.45, 11.4]],
+      [[2.0, 1.0]],
+      [[8.5, -5.0], [8.5, 2.31]]
+    ]
+
+human_3:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 0
+    goals: [ # Sequence provided by scenerio
+      [[2.75, -5.2], [8.64, 10.9]],
+      [[2.0, 1.0]],
+      [[10.0, 15.0], [10.0, 2.00]]
+    ]
+
+human_4:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 0
+    goals: [ # Sequence provided by scenerio
+      [[10.0, -0.4], [7.94, 19.4]],
+      [[2.0, 1.0]],
+      [[4.0, 1.4], [4.0, -1.4]]
+    ]
+
+human_5:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[4.0, -1.4], [4.0, 1.4]]
+    ]
+
+human_6:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[5.1, 21.6], [7.2, 10.2]]
+    ]
+
+human_7:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[7.2, 10.1], [5.1, 21.1]]
+    ]
+
+human_8:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[6.1, 1.6], [6.1, 14.1]]
+    ]
+
+human_9:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[6.0, 14.0], [6.0, 1.5]]
+    ]
+
+human_10:
+  loop_path: False
+  planner_name: 'gbl_planner/make_plan'
+  scenarios:
+    selected_id: 2
+    goals: [ # Sequence provided by scenerio
+      [[1.0, 1.0], [2.0, 2.0]],
+      [[2.0, 1.0]],
+      [[2.1, 2.1], [2.1, 14.1]]
+    ]