demo: add basic demo of solving rover domain with pyrobosim

cx_bringup/launch/cx_launch_demo.py

@@ -0,0 +1,116 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('cx_bringup')
+    cx_dir = get_package_share_directory('cx_clips_executive')
+
+    namespace = LaunchConfiguration('namespace')
+    cx_params_file = LaunchConfiguration('cx_params_file')
+    log_level = LaunchConfiguration('log_level')
+    model_file = LaunchConfiguration('model_file')
+
+    clips_executive_params_file = LaunchConfiguration(
+        'clips_executive_params_file')
+
+    lc_nodes = ["domain_expert", "problem_expert",
+                "planner", "clips_features_manager", "clips_executive"]
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
+
+    declare_model_file_cmd = DeclareLaunchArgument(
+        'model_file',
+        default_value=os.path.join(cx_dir + "/clips/demo-scenario/domain.pddl"),
+        description='PDDL Model file')
+
+    declare_log_level_ = DeclareLaunchArgument(
+        "log_level",
+        default_value='debug',
+        description="Logging level for cx_node executable",
+    )
+
+    declare_namespace_ = DeclareLaunchArgument(
+        'namespace', default_value='',
+        description='Default namespace')
+
+    declare_cx_params_file = DeclareLaunchArgument(
+        'cx_params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'cx_params.yaml'),
+        description='Path to the ROS2 cx_params.yaml file')
+
+    declare_clips_executive_params_file = DeclareLaunchArgument(
+        'clips_executive_params_file',
+        default_value=os.path.join(
+            bringup_dir, 'params', 'clips_executive.yaml'),
+        description='Path to Clips Executive params file')
+
+    plansys2_node_cmd = Node(
+        package='cx_bringup',
+        executable='plansys_node',
+        output='screen',
+        namespace=namespace,
+        parameters=[
+            {
+                'model_file': model_file,
+            },
+            cx_params_file
+        ])
+
+    cx_node = Node(
+        package='cx_bringup',
+        executable='cx_node',
+        output='screen',
+        emulate_tty=True,
+        namespace=namespace,
+        parameters=[
+            cx_params_file,
+            clips_executive_params_file
+        ],
+        arguments=['--ros-args', '--log-level', log_level]
+        # arguments=[('--ros-args --log-level debug')]
+    )
+
+    nav2_move_skill_node = Node(
+        package='cx_example_skill_nodes',
+        executable='skills_launch_node',
+        name='move_skill_node',
+        output='screen',
+        emulate_tty=True,
+        parameters=[]
+    )
+
+    cx_lifecycle_manager = Node(
+        package='cx_lifecycle_nodes_manager',
+        executable='lifecycle_manager_node',
+        name='cx_lifecycle_manager',
+        output='screen',
+        emulate_tty=True,
+        namespace=namespace,
+        parameters=[{"node_names_to_manage": lc_nodes}]
+    )
+
+    # The lauchdescription to populate with defined CMDS
+    ld = LaunchDescription()
+
+    ld.add_action(stdout_linebuf_envvar)
+    ld.add_action(declare_log_level_)
+
+    ld.add_action(declare_namespace_)
+    ld.add_action(declare_cx_params_file)
+    ld.add_action(declare_clips_executive_params_file)
+    ld.add_action(declare_model_file_cmd)
+
+    ld.add_action(nav2_move_skill_node)
+    ld.add_action(plansys2_node_cmd)
+    ld.add_action(cx_node)
+    ld.add_action(cx_lifecycle_manager)
+
+    return ld

cx_bringup/params/clips_executive.yaml

@@ -44,7 +44,7 @@ clips_executive:
 
     # Agent name. On startup the clips-agent will try to resolve a file named
     # <agent name>.clp. It must be in the CLIPS path directories.
-    spec: test-nav2
+    spec: demo
 
     specs:
       test:
@@ -301,3 +301,44 @@ clips_executive:
         action-mapping:
           say-hello: say{text="Hello ?(name)y", wait=true}
           say-goodbye: say{text="Good bye", wait=true}
+
+      demo:
+        parameters:
+          coordination:
+            mutex:
+              renew-interval: 5
+              max-age-sec: 15
+
+        init:
+          stage-1:
+            - name: plansys2
+            - name: clips_pddl_parser
+            - name: skill_execution
+          stage-3:
+            - name: psys2-planner
+              file: pddl.clp
+            - name: domain
+              file: demo-scenario/domain.clp
+            - name: worldmodel
+              file: demo-scenario/worldmodel-facts.clp
+            - name: goal-reasoner
+              file: demo-scenario/goal-reasoner.clp
+            - name: goal-expander
+              file: demo-scenario/goal-expander-pddl.clp
+            - name: action-selection
+              file: demo-scenario/action-selection.clp
+            - name: action-execution
+              files:
+                - demo-scenario/print-action.clp
+                - skills-actions.clp
+            - name: execution-monitoring
+              file: demo-scenario/execution-monitoring.clp
+            - name: state-estimation
+              file: demo-scenario/state-estimation.clp
+          # Map plan actions to skill strings.
+        action-mapping:
+          say-hello: say{text="Hello ?(name)y", wait=true}
+          say-goodbye: say{text="Good bye", wait=true}
+          navigate: navigate{}
+          sample_rock: sample_rock{}
+          sample_soil: sample_soil{}

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

@@ -0,0 +1,38 @@
+
+(defrule action-selection-select
+	?pa <- (plan-action (plan-id ?plan-id) (id ?id) (state FORMULATED)
+											(action-name ?action-name))
+	(plan (id ?plan-id) (goal-id ?goal-id))
+	(goal (id ?goal-id) (mode DISPATCHED))
+	(not (plan-action (state ~FORMULATED&~FINAL)))
+	(not (plan-action (state FORMULATED) (id ?oid&:(< ?oid ?id))))
+	=>
+	(modify ?pa (state PENDING))
+)
+
+(defrule action-selection-done
+	(plan (id ?plan-id) (goal-id ?goal-id))
+	?g <- (goal (id ?goal-id) (mode DISPATCHED))
+	(not (plan-action (plan-id ?plan-id) (state ~FINAL)))
+	=>
+	(modify ?g (mode FINISHED) (outcome COMPLETED))
+)
+
+(defrule action-selection-failed
+	(plan (id ?plan-id) (goal-id ?goal-id))
+	?g <- (goal (id ?goal-id) (mode DISPATCHED))
+	(plan-action (state FAILED))
+	=>
+	(modify ?g (mode FINISHED) (outcome FAILED))
+)
+
+(defrule action-execute-exogenous-noops
+  ?pa <- (plan-action (plan-id ?plan-id) (id ?id) (state PENDING)
+                   (action-name ?action&:(and (neq ?action navigate) (neq ?action sample_rock) (neq ?action sample_soil) (neq ?action drop)))
+                   ;(action-name ?action&:(neq ?action navigate))
+         (executable TRUE)
+         (param-values $?param-values))
+  =>
+  (printout t "Executing " ?action ?param-values crlf)
+  (modify ?pa (state EXECUTION-SUCCEEDED))
+)

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

@@ -0,0 +1,88 @@
+;---------------------------------------------------------------------------
+;  domain.clp -
+;  CX planning instance based on https://github.com/potassco/pddl-instances/blob/master/ipc-2002/domains/rovers-strips-automatic/instances/instance-1.pddl
+;---------------------------------------------------------------------------
+
+(defrule load-domain
+  (executive-init)
+  (not (domain-loaded))
+=>
+  (parse-pddl-domain (path-resolve "demo-scenario/domain.pddl"))
+  (assert (domain-loaded))
+)
+
+; (defrule test-domain-set-sensed-predicates
+;   (executive-init)
+;   (domain-loaded)
+;   ?p <- (domain-predicate (name robot_at) (sensed FALSE))
+; =>
+;   (modify ?p (sensed TRUE))
+;)
+
+(defrule load-initial-facts
+  (executive-init)
+  (domain-loaded)
+  =>
+  (assert (domain-fact (name visible) (param-values waypoint1 waypoint0)))
+  (assert (domain-fact (name visible) (param-values waypoint0 waypoint1)))
+  (assert (domain-fact (name visible) (param-values waypoint2 waypoint0)))
+  (assert (domain-fact (name visible) (param-values waypoint0 waypoint2)))
+  (assert (domain-fact (name visible) (param-values waypoint2 waypoint1)))
+  (assert (domain-fact (name visible) (param-values waypoint1 waypoint2)))
+  (assert (domain-fact (name visible) (param-values waypoint3 waypoint0)))
+  (assert (domain-fact (name visible) (param-values waypoint0 waypoint3)))
+  (assert (domain-fact (name visible) (param-values waypoint3 waypoint1)))
+  (assert (domain-fact (name visible) (param-values waypoint1 waypoint3)))
+  (assert (domain-fact (name visible) (param-values waypoint3 waypoint2)))
+  (assert (domain-fact (name visible) (param-values waypoint2 waypoint3)))
+  (assert (domain-fact (name at_soil_sample) (param-values waypoint0)))
+  (assert (domain-fact (name at_rock_sample) (param-values waypoint1)))
+  (assert (domain-fact (name at_soil_sample) (param-values waypoint2)))
+  (assert (domain-fact (name at_rock_sample) (param-values waypoint2)))
+  (assert (domain-fact (name at_soil_sample) (param-values waypoint3)))
+  (assert (domain-fact (name at_rock_sample) (param-values waypoint3)))
+  (assert (domain-fact (name at_lander) (param-values general waypoint0)))
+  (assert (domain-fact (name channel_free) (param-values general)))
+  (assert (domain-fact (name at) (param-values rover0 waypoint3)))
+  (assert (domain-fact (name available) (param-values rover0)))
+  (assert (domain-fact (name store_of) (param-values rover0store rover0)))
+  (assert (domain-fact (name empty) (param-values rover0store)))
+  (assert (domain-fact (name equipped_for_soil_analysis) (param-values rover0)))
+  (assert (domain-fact (name equipped_for_rock_analysis) (param-values rover0)))
+  (assert (domain-fact (name equipped_for_imaging) (param-values rover0)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint3 waypoint0)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint0 waypoint3)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint3 waypoint1)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint1 waypoint3)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint1 waypoint2)))
+  (assert (domain-fact (name can_traverse) (param-values rover0 waypoint2 waypoint1)))
+  (assert (domain-fact (name on_board) (param-values camera0 rover0)))
+  (assert (domain-fact (name calibration_target) (param-values camera0 objective1)))
+  (assert (domain-fact (name supports) (param-values camera0 colour)))
+  (assert (domain-fact (name supports) (param-values camera0 high_res)))
+  (assert (domain-fact (name visible_from) (param-values objective0 waypoint0)))
+  (assert (domain-fact (name visible_from) (param-values objective0 waypoint1)))
+  (assert (domain-fact (name visible_from) (param-values objective0 waypoint2)))
+  (assert (domain-fact (name visible_from) (param-values objective0 waypoint3)))
+  (assert (domain-fact (name visible_from) (param-values objective1 waypoint0)))
+  (assert (domain-fact (name visible_from) (param-values objective1 waypoint1)))
+  (assert (domain-fact (name visible_from) (param-values objective1 waypoint2)))
+  (assert (domain-fact (name visible_from) (param-values objective1 waypoint3)))
+  (assert
+          (domain-object (name general) (type lander))
+          (domain-object (name colour) (type mode))
+          (domain-object (name high_res) (type mode))
+          (domain-object (name low_res) (type mode))
+          (domain-object (name rover0) (type rover))
+          (domain-object (name rover0store) (type store))
+          (domain-object (name waypoint0) (type waypoint))
+          (domain-object (name waypoint1) (type waypoint))
+          (domain-object (name waypoint2) (type waypoint))
+          (domain-object (name waypoint3) (type waypoint))
+          (domain-object (name camera0) (type camera))
+          (domain-object (name objective0) (type objective))
+          (domain-object (name objective1) (type objective))
+  )
+
+  (assert (domain-facts-loaded))
+)