Skip to content

Commit 2cc5a81

Browse files
snoatosnoato
committed
demo: add basic demo of solving rover domain with pyrobosim
1 parent f4a5594 commit 2cc5a81

File tree

18 files changed

+1016
-8
lines changed

18 files changed

+1016
-8
lines changed

‎cx_bringup/launch/cx_launch_demo.py‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
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 = ["domain_expert", "problem_expert",
24+
"planner", "clips_features_manager", "clips_executive"]
25+
26+
stdout_linebuf_envvar = SetEnvironmentVariable(
27+
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
28+
29+
declare_model_file_cmd = DeclareLaunchArgument(
30+
'model_file',
31+
default_value=os.path.join(cx_dir + "/clips/demo-scenario/domain.pddl"),
32+
description='PDDL Model file')
33+
34+
declare_log_level_ = DeclareLaunchArgument(
35+
"log_level",
36+
default_value='debug',
37+
description="Logging level for cx_node executable",
38+
)
39+
40+
declare_namespace_ = DeclareLaunchArgument(
41+
'namespace', default_value='',
42+
description='Default namespace')
43+
44+
declare_cx_params_file = DeclareLaunchArgument(
45+
'cx_params_file',
46+
default_value=os.path.join(bringup_dir, 'params', 'cx_params.yaml'),
47+
description='Path to the ROS2 cx_params.yaml file')
48+
49+
declare_clips_executive_params_file = DeclareLaunchArgument(
50+
'clips_executive_params_file',
51+
default_value=os.path.join(
52+
bringup_dir, 'params', 'clips_executive.yaml'),
53+
description='Path to Clips Executive params file')
54+
55+
plansys2_node_cmd = Node(
56+
package='cx_bringup',
57+
executable='plansys_node',
58+
output='screen',
59+
namespace=namespace,
60+
parameters=[
61+
{
62+
'model_file': model_file,
63+
},
64+
cx_params_file
65+
])
66+
67+
cx_node = Node(
68+
package='cx_bringup',
69+
executable='cx_node',
70+
output='screen',
71+
emulate_tty=True,
72+
namespace=namespace,
73+
parameters=[
74+
cx_params_file,
75+
clips_executive_params_file
76+
],
77+
arguments=['--ros-args', '--log-level', log_level]
78+
# arguments=[('--ros-args --log-level debug')]
79+
)
80+
81+
nav2_move_skill_node = Node(
82+
package='cx_example_skill_nodes',
83+
executable='skills_launch_node',
84+
name='move_skill_node',
85+
output='screen',
86+
emulate_tty=True,
87+
parameters=[]
88+
)
89+
90+
cx_lifecycle_manager = Node(
91+
package='cx_lifecycle_nodes_manager',
92+
executable='lifecycle_manager_node',
93+
name='cx_lifecycle_manager',
94+
output='screen',
95+
emulate_tty=True,
96+
namespace=namespace,
97+
parameters=[{"node_names_to_manage": lc_nodes}]
98+
)
99+
100+
# The lauchdescription to populate with defined CMDS
101+
ld = LaunchDescription()
102+
103+
ld.add_action(stdout_linebuf_envvar)
104+
ld.add_action(declare_log_level_)
105+
106+
ld.add_action(declare_namespace_)
107+
ld.add_action(declare_cx_params_file)
108+
ld.add_action(declare_clips_executive_params_file)
109+
ld.add_action(declare_model_file_cmd)
110+
111+
ld.add_action(nav2_move_skill_node)
112+
ld.add_action(plansys2_node_cmd)
113+
ld.add_action(cx_node)
114+
ld.add_action(cx_lifecycle_manager)
115+
116+
return ld

‎cx_bringup/params/clips_executive.yaml‎

Lines changed: 42 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ 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: simple-agent
47+
spec: demo
4848

4949
specs:
5050
simple-agent:
@@ -329,3 +329,44 @@ clips_executive:
329329
action-mapping:
330330
say-hello: say{text="Hello ?(name)y", wait=true}
331331
say-goodbye: say{text="Good bye", wait=true}
332+
333+
demo:
334+
parameters:
335+
coordination:
336+
mutex:
337+
renew-interval: 5
338+
max-age-sec: 15
339+
340+
init:
341+
stage-1:
342+
- name: plansys2
343+
- name: clips_pddl_parser
344+
- name: skill_execution
345+
stage-3:
346+
- name: psys2-planner
347+
file: pddl.clp
348+
- name: domain
349+
file: demo-scenario/domain.clp
350+
- name: worldmodel
351+
file: demo-scenario/worldmodel-facts.clp
352+
- name: goal-reasoner
353+
file: demo-scenario/goal-reasoner.clp
354+
- name: goal-expander
355+
file: demo-scenario/goal-expander-pddl.clp
356+
- name: action-selection
357+
file: demo-scenario/action-selection.clp
358+
- name: action-execution
359+
files:
360+
- demo-scenario/print-action.clp
361+
- skills-actions.clp
362+
- name: execution-monitoring
363+
file: demo-scenario/execution-monitoring.clp
364+
- name: state-estimation
365+
file: demo-scenario/state-estimation.clp
366+
# Map plan actions to skill strings.
367+
action-mapping:
368+
say-hello: say{text="Hello ?(name)y", wait=true}
369+
say-goodbye: say{text="Good bye", wait=true}
370+
navigate: navigate{}
371+
sample_rock: sample_rock{}
372+
sample_soil: sample_soil{}

‎cx_clips_executive/src/cx_clips_executive/clips/demo-scenario/action-selection.clp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
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+
29+
(defrule action-execute-exogenous-noops
30+
?pa <- (plan-action (plan-id ?plan-id) (id ?id) (state PENDING)
31+
(action-name ?action&:(and (neq ?action navigate) (neq ?action sample_rock) (neq ?action sample_soil) (neq ?action drop)))
32+
;(action-name ?action&:(neq ?action navigate))
33+
(executable TRUE)
34+
(param-values $?param-values))
35+
=>
36+
(printout t "Executing " ?action ?param-values crlf)
37+
(modify ?pa (state EXECUTION-SUCCEEDED))
38+
)

‎cx_clips_executive/src/cx_clips_executive/clips/demo-scenario/domain.clp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
;---------------------------------------------------------------------------
2+
; domain.clp -
3+
; CX planning instance based on https://github.com/potassco/pddl-instances/blob/master/ipc-2002/domains/rovers-strips-automatic/instances/instance-1.pddl
4+
;---------------------------------------------------------------------------
5+
6+
(defrule load-domain
7+
(executive-init)
8+
(not (domain-loaded))
9+
=>
10+
(parse-pddl-domain (path-resolve "demo-scenario/domain.pddl"))
11+
(assert (domain-loaded))
12+
)
13+
14+
; (defrule test-domain-set-sensed-predicates
15+
; (executive-init)
16+
; (domain-loaded)
17+
; ?p <- (domain-predicate (name robot_at) (sensed FALSE))
18+
; =>
19+
; (modify ?p (sensed TRUE))
20+
;)
21+
22+
(defrule load-initial-facts
23+
(executive-init)
24+
(domain-loaded)
25+
=>
26+
(assert (domain-fact (name visible) (param-values waypoint1 waypoint0)))
27+
(assert (domain-fact (name visible) (param-values waypoint0 waypoint1)))
28+
(assert (domain-fact (name visible) (param-values waypoint2 waypoint0)))
29+
(assert (domain-fact (name visible) (param-values waypoint0 waypoint2)))
30+
(assert (domain-fact (name visible) (param-values waypoint2 waypoint1)))
31+
(assert (domain-fact (name visible) (param-values waypoint1 waypoint2)))
32+
(assert (domain-fact (name visible) (param-values waypoint3 waypoint0)))
33+
(assert (domain-fact (name visible) (param-values waypoint0 waypoint3)))
34+
(assert (domain-fact (name visible) (param-values waypoint3 waypoint1)))
35+
(assert (domain-fact (name visible) (param-values waypoint1 waypoint3)))
36+
(assert (domain-fact (name visible) (param-values waypoint3 waypoint2)))
37+
(assert (domain-fact (name visible) (param-values waypoint2 waypoint3)))
38+
(assert (domain-fact (name at_soil_sample) (param-values waypoint0)))
39+
(assert (domain-fact (name at_rock_sample) (param-values waypoint1)))
40+
(assert (domain-fact (name at_soil_sample) (param-values waypoint2)))
41+
(assert (domain-fact (name at_rock_sample) (param-values waypoint2)))
42+
(assert (domain-fact (name at_soil_sample) (param-values waypoint3)))
43+
(assert (domain-fact (name at_rock_sample) (param-values waypoint3)))
44+
(assert (domain-fact (name at_lander) (param-values general waypoint0)))
45+
(assert (domain-fact (name channel_free) (param-values general)))
46+
(assert (domain-fact (name at) (param-values rover0 waypoint3)))
47+
(assert (domain-fact (name available) (param-values rover0)))
48+
(assert (domain-fact (name store_of) (param-values rover0store rover0)))
49+
(assert (domain-fact (name empty) (param-values rover0store)))
50+
(assert (domain-fact (name equipped_for_soil_analysis) (param-values rover0)))
51+
(assert (domain-fact (name equipped_for_rock_analysis) (param-values rover0)))
52+
(assert (domain-fact (name equipped_for_imaging) (param-values rover0)))
53+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint3 waypoint0)))
54+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint0 waypoint3)))
55+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint3 waypoint1)))
56+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint1 waypoint3)))
57+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint1 waypoint2)))
58+
(assert (domain-fact (name can_traverse) (param-values rover0 waypoint2 waypoint1)))
59+
(assert (domain-fact (name on_board) (param-values camera0 rover0)))
60+
(assert (domain-fact (name calibration_target) (param-values camera0 objective1)))
61+
(assert (domain-fact (name supports) (param-values camera0 colour)))
62+
(assert (domain-fact (name supports) (param-values camera0 high_res)))
63+
(assert (domain-fact (name visible_from) (param-values objective0 waypoint0)))
64+
(assert (domain-fact (name visible_from) (param-values objective0 waypoint1)))
65+
(assert (domain-fact (name visible_from) (param-values objective0 waypoint2)))
66+
(assert (domain-fact (name visible_from) (param-values objective0 waypoint3)))
67+
(assert (domain-fact (name visible_from) (param-values objective1 waypoint0)))
68+
(assert (domain-fact (name visible_from) (param-values objective1 waypoint1)))
69+
(assert (domain-fact (name visible_from) (param-values objective1 waypoint2)))
70+
(assert (domain-fact (name visible_from) (param-values objective1 waypoint3)))
71+
(assert
72+
(domain-object (name general) (type lander))
73+
(domain-object (name colour) (type mode))
74+
(domain-object (name high_res) (type mode))
75+
(domain-object (name low_res) (type mode))
76+
(domain-object (name rover0) (type rover))
77+
(domain-object (name rover0store) (type store))
78+
(domain-object (name waypoint0) (type waypoint))
79+
(domain-object (name waypoint1) (type waypoint))
80+
(domain-object (name waypoint2) (type waypoint))
81+
(domain-object (name waypoint3) (type waypoint))
82+
(domain-object (name camera0) (type camera))
83+
(domain-object (name objective0) (type objective))
84+
(domain-object (name objective1) (type objective))
85+
)
86+
87+
(assert (domain-facts-loaded))
88+
)

0 commit comments

Comments
 (0)