minor update on BehaviorNavigationScenario (add menu)

Author: matthiasLutz

BehaviorNavigationScenario/model/BehaviorNavigationScenario.smartTcl

@@ -22,23 +22,23 @@
   :key '(is-a name) 
   :value '( 
             (is-a location)
-            (name 1)
-            (speech "ONE")
-            (approach-type (exact))
-            (approach-region-pose (800 0 0))
+            (name pose1)
+            (speech "pose-one")
+            (approach-type (region))
+            (approach-region-pose (-1000 2000 0))
             (approach-region-dist 150)
-            (approach-exact-pose (1000 0 0))
-            (approach-exact-dist 50)
+            (approach-exact-pose (600 1700 0))
+            (approach-exact-dist 100)
             (approach-exact-safetycl 0)
-            (orientation-region (position 0 0))
-            (orientation-exact (position 0 0))
+            (orientation-region (position 500 2000))
+            (orientation-exact (position 600 1900))
             (backward-dist 500)))
 
 (tcl-kb-update 
   :key '(is-a name) 
   :value '( 
             (is-a location)
-            (name 2)
+            (name pose2)
             (speech "TWO")
             (approach-region-pose (800 0 0))
             (approach-region-dist 150)
@@ -143,19 +143,19 @@
           (localizationModInst.tcb-set-robot-pose 0 0 0)
 
           (localizationModInst.tcb-activate-localization)
-          (nav1.tcb-move-robot 'approach-region 1)
-          (nav1.tcb-move-robot 'approach-exact 1)
-          (nav1.tcb-move-robot 'backward 1)
+          (nav1.tcb-move-robot 'approach-region pose1)
+          (nav1.tcb-move-robot 'approach-exact pose1)
+          (nav1.tcb-move-robot 'backward pose1)
           (nav1.tcb-move-robot 'backward-abs 100)
           (nav1.tcb-move-robot 'plain 1000 0 100)
-          (nav1.tcb-move-robot 'orientate-region 1)
-          (nav1.tcb-move-robot 'orientate-exact 1)
+          (nav1.tcb-move-robot 'orientate-region pose1)
+          (nav1.tcb-move-robot 'orientate-exact pose1)
           (nav1.tcb-move-robot 'approach-region home)
           (nav1.tcb-move-robot 'orientate-exact home)
           (nav1.tcb-move-robot 'orientate-angle-absolute 45)
           (nav1.tcb-move-robot 'orientate-angle-relative 135)
           (nav1.tcb-move-robot 'orientate-angle-relative 180)
-          (nav1.tcb-approach-location 1) ;region
+          (nav1.tcb-approach-location pose1) ;region
           (localizationModInst.tcb-deactivate-localization)
 
           (base.getBasePose => ?x ?y ?yaw)

BehaviorNavigationScenario/model/BehaviorNavigationScenario.taskRealization

@@ -3,8 +3,7 @@ TaskRealizationModel NavTasks {
 	AbstractCoordinationModuleInstance kbModInst coordModuleDef CommBasicObjects.KBModule
 	AbstractCoordinationModuleInstance localizationModInst coordModuleDef CommLocalizationObjects.LocalizationModule
 	AbstractCoordinationModuleInstance base coordModuleDef CommNavigationObjects.MobileBaseModule
-
-
-    //this task realization is a dummy (see smartTCL file)	
+	
+	//this task realization is a dummy (see smartTCL file)	
 	TaskRealization CommNavigationObjects.testTask { }
 }

BehaviorNavigationScenario/model/startUp.smartTcl

@@ -0,0 +1,65 @@
+(format t "INIT DONE ~%")
+
+(defun format-locations (s obj colonmod at-sign) (declare (ignore colonmod)) (declare (ignore at-sign)) (format s "~s" (get-value obj 'name)))
+
+;; MENU
+(defvar nmbr nil)
+
+(defun menu()
+  (execute '(nav1.tcb-init-navigation))
+  (execute '(nav1.tcb-load-navigation-map "navigation-map"))
+
+  (execute '(localizationModInst.tcb-load-localization-map "localization-map.yaml"))
+  (execute '(localizationModInst.tcb-trigger-global-localization))
+  (execute '(localizationModInst.tcb-set-robot-pose 0 0 0))
+(loop
+
+  (let ((known-locations (tcl-kb-query-all :key '(is-a) :value '((is-a location)))))
+
+  (format t "~%MENU~%====~%~%")
+  (format t " ---------------------------------------------------- ~%")
+  (format t " 3 - Start Joystick Navigation ~%")
+  (format t " 4 - Stop Joystick Navigation ~%")
+  (format t " ---------------------------------------------------- ~%")
+  (format t " 5 - Approach location ~%")
+  (format t " ---------------------------------------------------- ~%")
+  ;(format t " 95 - Add new locations ~%")
+  (format t " 96 - Show locations ~%")
+  ;(format t " 97 - Save locations to file ~%")
+  ;(format t " 98 - Load locations from file ~%")
+  (format t "99 - quit menu ~%")
+  (format t " 0 - exit ~%")
+  (setf nmbr (parse-integer (read-line nil)))
+  (format t "your choice: ~d ~%" nmbr)
+  
+  (cond
+    ((equal nmbr 3)
+      (execute '(nav1.tcb-joystick-navigation)))
+    ((equal nmbr 4)
+      (execute '(nav1.tcb-stop-joystick-navigation)))
+    ((equal nmbr 5)
+     (format t "Approach location - known locations: ~%")
+     (format t "~%~{~/format-locations/~^, ~}~%~%" known-locations)
+     (format t "Enter location name: ~%")
+     (setf locations-var (string-upcase (read-line nil)))
+     (setf locations-var (intern locations-var))
+     (format t "your choice: ~a ~%" locations-var)
+       (execute '(localizationModInst.tcb-activate-localization))
+       (execute `(nav1.tcb-approach-location ,locations-var))
+       (execute '(localizationModInst.tcb-deactivate-localization)))
+    ;((equal nmbr 95)
+     ;(add-locations))
+    ((equal nmbr 96)
+     (show-locations))
+    ;((equal nmbr 97)
+    ; (save-positions-from-kb-to-file))
+    ;((equal nmbr 98)
+    ; (load-positions-from-file-to-kb))
+    ((equal nmbr 99)
+      (format t "type (menu) to come back to menu ~%")
+      (return))
+    ((equal nmbr 0)
+      (format t "bye bye ~%")
+      (quit))))))
+
+(menu)