Skip to content

Commit 7fdb471

Browse files
renan028renan028
authored
Merge pull request #1 from MiRON-project/refactor_behavior_model
add space on the KB for the skill
2 parents da93f71 + 4915b8d commit 7fdb471

2 files changed

Lines changed: 157 additions & 31 deletions

File tree

‎BehaviorNavigationScenario/model/BehaviorNavigationScenario.smartTcl‎

Lines changed: 44 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,32 @@
11
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
22
;; SCENARIO MEMORY
33

4+
; ABR - Adding a space on the KB for the skill - Initial name: NIL
5+
(tcl-kb-update
6+
:key '(is-a)
7+
:value '(
8+
(is-a skill)
9+
(name NIL)
10+
(id -1)
11+
(in_1 NIL)
12+
(in_2 NIL)
13+
(in_3 NIL)
14+
(out_1 NIL)
15+
(out_2 NIL)
16+
(out_3 NIL)))
17+
18+
; ABR - Adding a space on the KB for the result of executing a skill - Initial name: NIL
19+
20+
(tcl-kb-update
21+
:key '(is-a)
22+
:value '(
23+
(is-a skill_result)
24+
(name NIL)
25+
(id -1)
26+
(result NIL)))
27+
28+
; positions
29+
;(tcl-kb-update :key '(is-a) :value '((is-a skill)(name NIL)) )
430
(tcl-kb-update
531
:key '(is-a name)
632
:value '(
@@ -41,9 +67,11 @@
4167
(name table)
4268
(speech "table")
4369
(approach-type (region))
44-
(approach-region-pose (2500 -2000 0))
70+
;(approach-region-pose (2500 -2000 0))
71+
(approach-region-pose (1500 -1000 0))
4572
(approach-region-dist 150)
46-
(approach-exact-pose (2500 -2000 0))
73+
;(approach-exact-pose (2500 -2000 0))
74+
(approach-exact-pose (1500 -1000 0))
4775
(approach-exact-dist 100)
4876
(approach-exact-safetycl 0)
4977
(orientation-region (position 2500 -3000))
@@ -141,7 +169,20 @@
141169
(format t " orientation : ~s ~%" (get-value obj 'orientation))
142170
(format t " station-distance : ~s ~%" (get-value obj 'station-distance))
143171
(format t " belt-count : ~s ~%" (get-value obj 'belt-count)))))
144-
172+
173+
(defun show-skills ()
174+
(let ((obj-list (tcl-kb-query-all :key '(is-a) :value '((is-a skill)))))
175+
(dolist (obj obj-list)
176+
(format t "~%--------------------------------~%")
177+
(format t "SKILL ------------------~%")
178+
(format t "name : ~s ~%" (get-value obj 'name))
179+
(format t " in_1 : ~s ~%" (get-value obj 'in_1))
180+
(format t " in_2 : ~s ~%" (get-value obj 'in_2))
181+
(format t " in_3 : ~s ~%" (get-value obj 'in_3))
182+
(format t " out_1 : ~s ~%" (get-value obj 'out_1))
183+
(format t " out_2 : ~s ~%" (get-value obj 'out_2))
184+
(format t " out_3 : ~s ~%" (get-value obj 'out_3)))))
185+
145186

146187
(defun show-robots ()
147188
(let ((robot-list (tcl-kb-query-all :key '(is-a) :value '((is-a robot)))))

‎BehaviorNavigationScenario/model/startUp.smartTcl‎

Lines changed: 113 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,21 @@
22

33
(defun format-locations (s obj colonmod at-sign) (declare (ignore colonmod)) (declare (ignore at-sign)) (format s "~s" (get-value obj 'name)))
44

5+
;;;;;;;;;;;;;;;;;;;;;
6+
57
;; MENU
68
(defvar nmbr nil)
9+
;
10+
; capture the skill
11+
(defvar name_skill NIL)
12+
(defvar in_1_skill NIL)
13+
(defvar in_2_skill NIL)
14+
(defvar in_3_skill NIL)
15+
(defvar out_1_skill NIL)
16+
(defvar out_2_skill NIL)
17+
(defvar out_3_skill NIL)
18+
;(defvar id_skill -1)
19+
;
720

821
(defun menu()
922
(execute '(nav1.tcb-init-navigation))
@@ -23,43 +36,115 @@
2336
(format t " ---------------------------------------------------- ~%")
2437
(format t " 5 - Approach location ~%")
2538
(format t " ---------------------------------------------------- ~%")
26-
;(format t " 95 - Add new locations ~%")
39+
(format t " 6 - Move robot plain x y angle ~%") ;case 6 - ABR
40+
(format t " ---------------------------------------------------- ~%")
41+
;(format t " 95 - Add new locations ~%") ;ABR
42+
(format t " 95 - Show skills ~%")
2743
(format t " 96 - Show locations ~%")
2844
;(format t " 97 - Save locations to file ~%")
2945
;(format t " 98 - Load locations from file ~%")
3046
(format t "99 - quit menu ~%")
3147
(format t " 0 - exit ~%")
32-
(setf nmbr (parse-integer (read-line nil)))
33-
(format t "your choice: ~d ~%" nmbr)
3448

35-
(cond
36-
((equal nmbr 3)
37-
(execute '(nav1.tcb-joystick-navigation)))
38-
((equal nmbr 4)
39-
(execute '(nav1.tcb-stop-joystick-navigation)))
40-
((equal nmbr 5)
41-
(format t "Approach location - known locations: ~%")
42-
(format t "~%~{~/format-locations/~^, ~}~%~%" known-locations)
43-
(format t "Enter location name: ~%")
44-
(setf locations-var (string-upcase (read-line nil)))
45-
(setf locations-var (intern locations-var))
46-
(format t "your choice: ~a ~%" locations-var)
47-
(execute '(localizationModInst.tcb-activate-localization))
48-
(execute `(nav1.tcb-approach-location ,locations-var))
49-
(execute '(localizationModInst.tcb-deactivate-localization)))
50-
;((equal nmbr 95)
51-
;(add-locations))
52-
((equal nmbr 96)
53-
(show-locations))
49+
50+
; ABR - Reading a skill
51+
(let ((obj_skill (tcl-kb-query :key '(is-a) :value '((is-a skill)) ) ))
52+
(setf name_skill (get-value obj_skill 'name)) ; read the name of the skill to execute
53+
(setf in_1_skill (get-value obj_skill 'in_1)) ; read the parameter IN1 of the skill to execute
54+
(setf in_2_skill (get-value obj_skill 'in_2)) ; read the parameter IN2 of the skill to execute
55+
(setf in_3_skill (get-value obj_skill 'in_3)) ; read the parameter IN3 of the skill to execute
56+
(setf out_1_skill (get-value obj_skill 'out_1)) ; read the parameter OUT1 of the skill to execute
57+
(setf out_2_skill (get-value obj_skill 'out_2)) ; read the parameter OUT2 of the skill to execute
58+
(setf out_3_skill (get-value obj_skill 'out_3)) ; read the parameter OUT3 of the skill to execute
59+
(format t "---> name : ~s ~%" name_skill)
60+
(cond
61+
((string-equal name_skill "NIL") ;
62+
)
63+
((string-equal name_skill "ACTIVATELOCALIZATION")
64+
(execute '(localizationModInst.tcb-activate-localization))
65+
(tcl-kb-update :key '(is-a) :value '((is-a skill_result)(name ACTIVATELOCALIZATION)) ) ;add id_skill
66+
)
67+
((string-equal name_skill "APPROACHLOCATION")
68+
(format t "------> DENTRO : ~s ~d ~%" name_skill in_1_skill)
69+
(setf locations-var "HOME")
70+
(cond
71+
((equal in_1_skill 1.0)
72+
(setf locations-var "HOME"))
73+
((equal in_1_skill 2.0)
74+
(setf locations-var "SOFA"))
75+
((equal in_1_skill 3.0)
76+
(setf locations-var "TABLE"))
77+
((equal in_1_skill 4.0)
78+
(setf locations-var "BENCH"))
79+
)
80+
(setf locations-var (intern locations-var))
81+
(execute `(nav1.tcb-approach-location ,locations-var))
82+
(tcl-kb-update :key '(is-a) :value '((is-a skill_result)(name APPROACHLOCATION)) )
83+
)
84+
((string-equal name_skill "DEACTIVATELOCALIZATION")
85+
(execute '(localizationModInst.tcb-deactivate-localization))
86+
(tcl-kb-update :key '(is-a) :value '((is-a skill_result)(name DEACTIVATELOCALIZATION)) )
87+
)
88+
)
89+
90+
;(format t "~%--------------------------------~%")
91+
;(format t "SKILL ------------------~%")
92+
;(format t "name : ~s ~%" (get-value obj_skill 'name))
93+
;(format t " in_1 : ~s ~%" (get-value obj_skill 'in_1))
94+
;(format t " in_2 : ~s ~%" (get-value obj_skill 'in_2))
95+
;(format t " in_3 : ~s ~%" (get-value obj_skill 'in_3))
96+
;(format t " out_1 : ~s ~%" (get-value obj_skill 'out_1))
97+
;(format t " out_2 : ~s ~%" (get-value obj_skill 'out_2))
98+
;(format t " out_3 : ~s ~%" (get-value obj_skill 'out_3))
99+
)
100+
101+
102+
103+
104+
105+
106+
; (setf nmbr (parse-integer (read-line nil)))
107+
; (format t "your choice: ~d ~%" nmbr)
108+
109+
; (cond
110+
; ((equal nmbr 3)
111+
; (execute '(nav1.tcb-joystick-navigation)))
112+
; ((equal nmbr 4)
113+
; (execute '(nav1.tcb-stop-joystick-navigation)))
114+
; ((equal nmbr 5)
115+
; (format t "Approach location - known locations: ~%")
116+
; (format t "~%~{~/format-locations/~^, ~}~%~%" known-locations)
117+
; (format t "Enter location name: ~%")
118+
; (setf locations-var (string-upcase (read-line nil)))
119+
; (setf locations-var (intern locations-var))
120+
; (format t "your choice: ~a ~%" locations-var)
121+
; (execute '(localizationModInst.tcb-activate-localization))
122+
; (execute `(nav1.tcb-approach-location ,locations-var)) ;ABR - recuperar
123+
; (execute '(localizationModInst.tcb-deactivate-localization)))
124+
; ((equal nmbr 6) ; case 6 - ABR
125+
; (format t " x position ~%")
126+
; (setf xpos (parse-integer (read-line nil)))
127+
; (format t " y position ~%")
128+
; (setf ypos (parse-integer (read-line nil)))
129+
; (execute '(localizationModInst.tcb-activate-localization))
130+
; (execute `(nav1.tcb-move-robot 'plain ,xpos ,ypos 100)) ;ABR - tocando con move_robot
131+
; (execute '(localizationModInst.tcb-deactivate-localization))) ;end-case 6 - ABR
132+
;((equal nmbr 95) ;ABR - undefined function
133+
;(add-locations)) ;ABR
134+
; ((equal nmbr 95)
135+
; (show-skills))
136+
; ((equal nmbr 96)
137+
; (show-locations))
54138
;((equal nmbr 97)
55139
; (save-positions-from-kb-to-file))
56140
;((equal nmbr 98)
57141
; (load-positions-from-file-to-kb))
58-
((equal nmbr 99)
59-
(format t "type (menu) to come back to menu ~%")
60-
(return))
61-
((equal nmbr 0)
62-
(format t "bye bye ~%")
63-
(quit))))))
142+
; ((equal nmbr 99)
143+
; (format t "type (menu) to come back to menu ~%")
144+
; (return))
145+
; ((equal nmbr 0)
146+
; (format t "bye bye ~%")
147+
; (quit)))
148+
)))
64149

65150
(menu)

0 commit comments

Comments
 (0)