Skip to content

Commit 8d51820

Browse files
snoatosnoato
committed
cx: add simple agent for tests with robots
1 parent ef89d54 commit 8d51820

File tree

8 files changed

+1350
-1
lines changed

8 files changed

+1350
-1
lines changed

‎cx_bringup/launch/cx_launch_simple_agent.py‎

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
import os
2+
3+
from ament_index_python.packages import get_package_share_directory
4+
5+
from launch import LaunchDescription
6+
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
7+
from launch.substitutions import LaunchConfiguration
8+
from launch_ros.actions import Node
9+
10+
11+
def generate_launch_description():
12+
bringup_dir = get_package_share_directory('cx_bringup')
13+
cx_dir = get_package_share_directory('cx_clips_executive')
14+
15+
namespace = LaunchConfiguration('namespace')
16+
cx_params_file = LaunchConfiguration('cx_params_file')
17+
log_level = LaunchConfiguration('log_level')
18+
model_file = LaunchConfiguration('model_file')
19+
20+
clips_executive_params_file = LaunchConfiguration(
21+
'clips_executive_params_file')
22+
23+
lc_nodes = ["clips_features_manager", "clips_executive"]
24+
25+
stdout_linebuf_envvar = SetEnvironmentVariable(
26+
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
27+
28+
declare_model_file_cmd = DeclareLaunchArgument(
29+
'model_file',
30+
default_value=os.path.join(cx_dir + "/clips/simple-agent/domain.pddl"),
31+
description='PDDL Model file')
32+
33+
declare_log_level_ = DeclareLaunchArgument(
34+
"log_level",
35+
default_value='debug',
36+
description="Logging level for cx_node executable",
37+
)
38+
39+
declare_namespace_ = DeclareLaunchArgument(
40+
'namespace', default_value='',
41+
description='Default namespace')
42+
43+
declare_cx_params_file = DeclareLaunchArgument(
44+
'cx_params_file',
45+
default_value=os.path.join(bringup_dir, 'params', 'cx_params.yaml'),
46+
description='Path to the ROS2 cx_params.yaml file')
47+
48+
declare_clips_executive_params_file = DeclareLaunchArgument(
49+
'clips_executive_params_file',
50+
default_value=os.path.join(
51+
bringup_dir, 'params', 'clips_executive.yaml'),
52+
description='Path to Clips Executive params file')
53+
54+
cx_node = Node(
55+
package='cx_bringup',
56+
executable='cx_node',
57+
output='screen',
58+
emulate_tty=True,
59+
namespace=namespace,
60+
parameters=[
61+
cx_params_file,
62+
clips_executive_params_file
63+
],
64+
arguments=['--ros-args', '--log-level', log_level]
65+
# arguments=[('--ros-args --log-level debug')]
66+
)
67+
68+
nav2_move_skill_node = Node(
69+
package='cx_example_skill_nodes',
70+
executable='skills_launch_node',
71+
name='skills_node',
72+
output='screen',
73+
emulate_tty=True,
74+
parameters=[]
75+
)
76+
77+
cx_lifecycle_manager = Node(
78+
package='cx_lifecycle_nodes_manager',
79+
executable='lifecycle_manager_node',
80+
name='cx_lifecycle_manager',
81+
output='screen',
82+
emulate_tty=True,
83+
namespace=namespace,
84+
parameters=[{"node_names_to_manage": lc_nodes}]
85+
)
86+
87+
# The lauchdescription to populate with defined CMDS
88+
ld = LaunchDescription()
89+
90+
ld.add_action(stdout_linebuf_envvar)
91+
ld.add_action(declare_log_level_)
92+
93+
ld.add_action(declare_namespace_)
94+
ld.add_action(declare_cx_params_file)
95+
ld.add_action(declare_clips_executive_params_file)
96+
ld.add_action(declare_model_file_cmd)
97+
98+
ld.add_action(nav2_move_skill_node)
99+
ld.add_action(cx_node)
100+
ld.add_action(cx_lifecycle_manager)
101+
102+
return ld

‎cx_bringup/params/clips_executive.yaml‎

Lines changed: 29 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,9 +44,37 @@ clips_executive:
4444

4545
# Agent name. On startup the clips-agent will try to resolve a file named
4646
# <agent name>.clp. It must be in the CLIPS path directories.
47-
spec: test-nav2
47+
spec: simple-agent
4848

4949
specs:
50+
simple-agent:
51+
parameters:
52+
coordination:
53+
mutex:
54+
renew-interval: 5
55+
max-age-sec: 15
56+
57+
init:
58+
stage-1:
59+
- name: clips_pddl_parser
60+
- name: skill_execution
61+
stage-3:
62+
- name: domain
63+
file: simple-agent/domain.clp
64+
- name: goal-reasoner
65+
file: simple-agent/goal-reasoner.clp
66+
- name: fixed-sequence
67+
file: simple-agent/fixed-sequence.clp
68+
- name: action-selection
69+
file: simple-agent/action-selection.clp
70+
- name: action-execution
71+
files:
72+
- simple-agent/print-action.clp
73+
- skills-actions.clp
74+
# Map plan actions to skill strings.
75+
action-mapping:
76+
gowait: skill{}
77+
5078
test:
5179
# The following configures CX initialization.
5280
# Initialization is organized in three stages:

‎cx_clips_executive/src/cx_clips_executive/clips/simple-agent/action-selection.clp‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
2+
(defrule action-selection-select
3+
?pa <- (plan-action (plan-id ?plan-id) (id ?id) (state FORMULATED)
4+
(action-name ?action-name))
5+
(plan (id ?plan-id) (goal-id ?goal-id))
6+
(goal (id ?goal-id) (mode DISPATCHED))
7+
(not (plan-action (state ~FORMULATED&~FINAL)))
8+
(not (plan-action (state FORMULATED) (id ?oid&:(< ?oid ?id))))
9+
=>
10+
(modify ?pa (state PENDING))
11+
)
12+
13+
(defrule action-selection-done
14+
(plan (id ?plan-id) (goal-id ?goal-id))
15+
?g <- (goal (id ?goal-id) (mode DISPATCHED))
16+
(not (plan-action (plan-id ?plan-id) (state ~FINAL)))
17+
=>
18+
(modify ?g (mode FINISHED) (outcome COMPLETED))
19+
)
20+
21+
(defrule action-selection-failed
22+
(plan (id ?plan-id) (goal-id ?goal-id))
23+
?g <- (goal (id ?goal-id) (mode DISPATCHED))
24+
(plan-action (state FAILED))
25+
=>
26+
(modify ?g (mode FINISHED) (outcome FAILED))
27+
)
28+

‎cx_clips_executive/src/cx_clips_executive/clips/simple-agent/domain.clp‎

Lines changed: 225 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,225 @@
1+
(defrule load-domain
2+
(executive-init)
3+
(not (domain-loaded))
4+
=>
5+
(parse-pddl-domain (path-resolve "simple-agent/domain.pddl"))
6+
(assert (domain-loaded))
7+
)
8+
9+
(defrule domain-set-sensed-predicates
10+
" Mark some predicates as sensed predicates.
11+
That means, the truth value of these predicates can be changed not directly but by some external trigger
12+
"
13+
(domain-loaded)
14+
?p <- (domain-predicate (name mps-state|zone-content) (sensed FALSE))
15+
=>
16+
(modify ?p (sensed TRUE))
17+
)
18+
19+
20+
(defrule domain-set-value-predicates
21+
?p <- (domain-predicate (name mps-state|zone-content) (value-predicate FALSE))
22+
=>
23+
(modify ?p (value-predicate TRUE))
24+
)
25+
26+
27+
(defrule domain-nowait-actions
28+
" Mark some actions that have a sensed effect as non-waiting. That means the effect is applied without sensing for it "
29+
(domain-loaded)
30+
?o <- (domain-operator (name wp-put|prepare-bs|prepare-rs|prepare-ds|prepare-cs|location-unlock) (wait-sensed ~FALSE))
31+
=>
32+
(modify ?o (wait-sensed FALSE))
33+
)
34+
35+
36+
(defrule domain-exogenous-actions
37+
"Mark all actions, that model state changes of the machines, as exogenous"
38+
?op <- (domain-operator
39+
(name bs-dispense | cs-mount-cap | cs-retrieve-cap | rs-mount-ring1 |
40+
rs-mount-ring2 | rs-mount-ring3 | fulfill-order-c0 | fulfill-order-discard |
41+
fulfill-order-c1 | fulfill-order-c2 | fulfill-order-c3 | ss-retrieve-c0)
42+
(exogenous FALSE)
43+
)
44+
=>
45+
(modify ?op (exogenous TRUE))
46+
)
47+
48+
(deffunction domain-load-local-facts (?team-color)
49+
" Initialize facts that are not synced."
50+
(if (eq ?team-color CYAN)
51+
then
52+
(bind ?bs C-BS)
53+
(bind ?cs1 C-CS1)
54+
(bind ?cs2 C-CS2)
55+
(bind ?rs1 C-RS1)
56+
(bind ?rs2 C-RS2)
57+
(bind ?ds C-DS)
58+
(bind ?ss C-SS)
59+
else
60+
(bind ?bs M-BS)
61+
(bind ?cs1 M-CS1)
62+
(bind ?cs2 M-CS2)
63+
(bind ?rs1 M-RS1)
64+
(bind ?rs2 M-RS2)
65+
(bind ?ds M-DS)
66+
(bind ?ss M-SS)
67+
)
68+
(assert
69+
(domain-fact (name at) (param-values robot1 START INPUT))
70+
(domain-fact (name at) (param-values robot2 START INPUT))
71+
(domain-fact (name at) (param-values robot3 START INPUT))
72+
(domain-fact (name mps-team) (param-values ?bs ?team-color))
73+
(domain-fact (name can-hold) (param-values robot1))
74+
(domain-fact (name can-hold) (param-values robot2))
75+
(domain-fact (name can-hold) (param-values robot3))
76+
(domain-fact (name mps-team) (param-values ?ds ?team-color))
77+
(domain-fact (name mps-team) (param-values ?ss ?team-color))
78+
(domain-fact (name mps-team) (param-values ?cs1 ?team-color))
79+
(domain-fact (name mps-team) (param-values ?cs2 ?team-color))
80+
(domain-fact (name mps-team) (param-values ?rs1 ?team-color))
81+
(domain-fact (name mps-team) (param-values ?rs2 ?team-color))
82+
83+
(domain-fact (name mps-type) (param-values C-BS BS))
84+
(domain-fact (name mps-type) (param-values C-DS DS))
85+
(domain-fact (name mps-type) (param-values C-SS SS))
86+
(domain-fact (name mps-type) (param-values C-CS1 CS))
87+
(domain-fact (name mps-type) (param-values C-CS2 CS))
88+
(domain-fact (name mps-type) (param-values C-RS1 RS))
89+
(domain-fact (name mps-type) (param-values C-RS2 RS))
90+
91+
(domain-fact (name mps-type) (param-values M-BS BS))
92+
(domain-fact (name mps-type) (param-values M-DS DS))
93+
(domain-fact (name mps-type) (param-values M-SS SS))
94+
(domain-fact (name mps-type) (param-values M-CS1 CS))
95+
(domain-fact (name mps-type) (param-values M-CS2 CS))
96+
(domain-fact (name mps-type) (param-values M-RS1 RS))
97+
(domain-fact (name mps-type) (param-values M-RS2 RS))
98+
(domain-fact (name rs-sub) (param-values THREE TWO ONE))
99+
(domain-fact (name rs-sub) (param-values THREE ONE TWO))
100+
(domain-fact (name rs-sub) (param-values THREE ZERO THREE))
101+
(domain-fact (name rs-sub) (param-values TWO TWO ZERO))
102+
(domain-fact (name rs-sub) (param-values TWO ONE ONE))
103+
(domain-fact (name rs-sub) (param-values TWO ZERO TWO))
104+
(domain-fact (name rs-sub) (param-values ONE ONE ZERO))
105+
(domain-fact (name rs-sub) (param-values ONE ZERO ONE))
106+
(domain-fact (name rs-sub) (param-values ZERO ZERO ZERO))
107+
(domain-fact (name rs-inc) (param-values ZERO ONE))
108+
(domain-fact (name rs-inc) (param-values ONE TWO))
109+
(domain-fact (name rs-inc) (param-values TWO THREE))
110+
(domain-fact (name cs-color) (param-values ?cs1 CAP_GREY))
111+
(domain-fact (name cs-color) (param-values ?cs2 CAP_BLACK))
112+
113+
(domain-fact (name mirror-orientation) (param-values 0 180))
114+
(domain-fact (name mirror-orientation) (param-values 45 135))
115+
(domain-fact (name mirror-orientation) (param-values 90 90))
116+
(domain-fact (name mirror-orientation) (param-values 135 45))
117+
(domain-fact (name mirror-orientation) (param-values 180 0))
118+
(domain-fact (name mirror-orientation) (param-values 225 315))
119+
(domain-fact (name mirror-orientation) (param-values 270 270))
120+
(domain-fact (name mirror-orientation) (param-values 315 225))
121+
122+
(domain-object (name CCB1) (type cap-carrier))
123+
(domain-object (name CCB2) (type cap-carrier))
124+
(domain-object (name CCB3) (type cap-carrier))
125+
(domain-object (name CCG1) (type cap-carrier))
126+
(domain-object (name CCG2) (type cap-carrier))
127+
(domain-object (name CCG3) (type cap-carrier))
128+
(domain-object (name ?bs) (type mps))
129+
(domain-object (name ?cs1) (type mps))
130+
(domain-object (name ?cs2) (type mps))
131+
(domain-object (name ?ds) (type mps))
132+
(domain-object (name ?rs1) (type mps))
133+
(domain-object (name ?rs2) (type mps))
134+
(domain-object (name ?ss) (type mps))
135+
(domain-object (name INPUT) (type mps-side))
136+
(domain-object (name OUTPUT) (type mps-side))
137+
(domain-object (name WAIT) (type mps-side))
138+
(domain-object (name ?team-color) (type team-color))
139+
(domain-object (name O1) (type order))
140+
(domain-object (name O2) (type order))
141+
(domain-object (name O3) (type order))
142+
(domain-object (name O4) (type order))
143+
(domain-object (name O5) (type order))
144+
(domain-object (name O6) (type order))
145+
(domain-object (name O7) (type order))
146+
(domain-object (name O8) (type order))
147+
(domain-object (name O9) (type order))
148+
(domain-fact (name rs-ring-spec) (param-values ?rs1 RING_NONE ZERO))
149+
(domain-fact (name rs-ring-spec) (param-values ?rs2 RING_NONE ZERO))
150+
)
151+
)
152+
153+
(defrule domain-load-initial-facts
154+
" Load all initial domain facts on startup of the game "
155+
(domain-loaded)
156+
=>
157+
(printout info "Initializing worldmodel" crlf)
158+
(bind ?team-color MAGENTA)
159+
(if (eq ?team-color CYAN)
160+
then
161+
(bind ?bs C-BS)
162+
(bind ?cs1 C-CS1)
163+
(bind ?cs2 C-CS2)
164+
(bind ?rs1 C-RS1)
165+
(bind ?rs2 C-RS2)
166+
(bind ?ds C-DS)
167+
(bind ?ss C-SS)
168+
else
169+
(bind ?bs M-BS)
170+
(bind ?cs1 M-CS1)
171+
(bind ?cs2 M-CS2)
172+
(bind ?rs1 M-RS1)
173+
(bind ?rs2 M-RS2)
174+
(bind ?ds M-DS)
175+
(bind ?ss M-SS)
176+
)
177+
178+
(domain-load-local-facts ?team-color)
179+
(foreach ?mps (create$ ?bs ?cs1 ?cs2 ?rs1 ?rs2 ?ds ?ss)
180+
(assert
181+
(domain-fact (name mps-side-free) (param-values ?mps INPUT))
182+
(domain-fact (name mps-side-free) (param-values ?mps OUTPUT))
183+
(domain-fact (name mps-side-approachable) (param-values ?mps INPUT))
184+
(domain-fact (name mps-side-approachable) (param-values ?mps OUTPUT))
185+
(domain-fact (name mps-side-approachable) (param-values ?mps WAIT))
186+
)
187+
)
188+
(foreach ?robot (create$ robot1 robot2 robot3)
189+
(assert
190+
(domain-fact (name at) (param-values ?robot START INPUT))
191+
(domain-fact (name can-hold) (param-values ?robot))
192+
)
193+
)
194+
(foreach ?cc (create$ CCB1 CCB2 CCB3 CCG1 CCG2 CCG3)
195+
(assert
196+
(domain-fact (name wp-base-color) (param-values ?cc BASE_CLEAR))
197+
(domain-fact (name wp-ring1-color) (param-values ?cc RING_NONE))
198+
(domain-fact (name wp-ring2-color) (param-values ?cc RING_NONE))
199+
(domain-fact (name wp-ring3-color) (param-values ?cc RING_NONE))
200+
)
201+
)
202+
(assert
203+
(domain-fact (name wp-cap-color) (param-values CCB1 CAP_BLACK))
204+
(domain-fact (name wp-cap-color) (param-values CCB2 CAP_BLACK))
205+
(domain-fact (name wp-cap-color) (param-values CCB3 CAP_BLACK))
206+
(domain-fact (name wp-cap-color) (param-values CCG1 CAP_GREY))
207+
(domain-fact (name wp-cap-color) (param-values CCG2 CAP_GREY))
208+
(domain-fact (name wp-cap-color) (param-values CCG3 CAP_GREY))
209+
(domain-fact (name wp-on-shelf) (param-values CCB1 ?cs2 LEFT))
210+
(domain-fact (name wp-on-shelf) (param-values CCB2 ?cs2 MIDDLE))
211+
(domain-fact (name wp-on-shelf) (param-values CCB3 ?cs2 RIGHT))
212+
(domain-fact (name wp-on-shelf) (param-values CCG1 ?cs1 LEFT))
213+
(domain-fact (name wp-on-shelf) (param-values CCG2 ?cs1 MIDDLE))
214+
(domain-fact (name wp-on-shelf) (param-values CCG3 ?cs1 RIGHT))
215+
(domain-fact (name cs-can-perform) (param-values ?cs1 RETRIEVE_CAP))
216+
(domain-fact (name cs-can-perform) (param-values ?cs2 RETRIEVE_CAP))
217+
(domain-fact (name cs-free) (param-values ?cs1))
218+
(domain-fact (name cs-free) (param-values ?cs2))
219+
(domain-fact (name cs-color) (param-values ?cs1 CAP_GREY))
220+
(domain-fact (name cs-color) (param-values ?cs2 CAP_BLACK))
221+
(domain-fact (name rs-filled-with) (param-values ?rs1 ZERO))
222+
(domain-fact (name rs-filled-with) (param-values ?rs2 ZERO))
223+
)
224+
(assert (domain-facts-loaded))
225+
)

0 commit comments

Comments
 (0)