From 16d9043449661c9dbf7f2ed460ac89c9715562de Mon Sep 17 00:00:00 2001 From: Matthias Lutz Date: Thu, 29 Aug 2019 16:09:10 +0200 Subject: [PATCH] minor update on BehaviorNavigationScenario (add menu) --- .../model/BehaviorNavigationScenario.smartTcl | 30 ++++----- ...BehaviorNavigationScenario.taskRealization | 5 +- .../model/startUp.smartTcl | 65 +++++++++++++++++++ 3 files changed, 82 insertions(+), 18 deletions(-) create mode 100644 BehaviorNavigationScenario/model/startUp.smartTcl diff --git a/BehaviorNavigationScenario/model/BehaviorNavigationScenario.smartTcl b/BehaviorNavigationScenario/model/BehaviorNavigationScenario.smartTcl index dd99124..bcb79c5 100644 --- a/BehaviorNavigationScenario/model/BehaviorNavigationScenario.smartTcl +++ b/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) diff --git a/BehaviorNavigationScenario/model/BehaviorNavigationScenario.taskRealization b/BehaviorNavigationScenario/model/BehaviorNavigationScenario.taskRealization index a59fa07..42a9702 100644 --- a/BehaviorNavigationScenario/model/BehaviorNavigationScenario.taskRealization +++ b/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 { } } \ No newline at end of file diff --git a/BehaviorNavigationScenario/model/startUp.smartTcl b/BehaviorNavigationScenario/model/startUp.smartTcl new file mode 100644 index 0000000..4d9889b --- /dev/null +++ b/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)