Skip to content

Error on executor node: requirement not met #394

@MattiaTinfena

Description

@MattiaTinfena

I'm having an issue computing the behavior tree. After successfully generated the plan, when I try to execute plansys2 it returns this error: [executor_node-4] [ERROR] requirement not met: [(= (num_id_detected) (num_marker))]. The fact is that if i remove that part of the plan it works correctly even tho in the code there is (at start(= (num_photo_taken) (num_marker))) which is exactly the same instruction.

Ros version: jazzy
Ubuntu version: 24.04
Plansys2 installed with: sudo apt install ros-jazzy-plansys2-*
Plansys2 installed packages:

ros-jazzy-plansys2-bringup-dbgsym/noble,now 2.0.18-1noble.20251108.001318 amd64 [installed]
ros-jazzy-plansys2-bringup/noble,now 2.0.18-1noble.20251108.001318 amd64 [installed]
ros-jazzy-plansys2-bt-actions-dbgsym/noble,now 2.0.18-1noble.20251026.063040 amd64 [installed]
ros-jazzy-plansys2-bt-actions/noble,now 2.0.18-1noble.20251026.063040 amd64 [installed]
ros-jazzy-plansys2-core-dbgsym/noble,now 2.0.18-1noble.20251025.095353 amd64 [installed]
ros-jazzy-plansys2-core/noble,now 2.0.18-1noble.20251025.095353 amd64 [installed]
ros-jazzy-plansys2-domain-expert-dbgsym/noble,now 2.0.18-1noble.20251025.095845 amd64 [installed]
ros-jazzy-plansys2-domain-expert/noble,now 2.0.18-1noble.20251025.095845 amd64 [installed]
ros-jazzy-plansys2-executor-dbgsym/noble,now 2.0.18-1noble.20251026.062051 amd64 [installed]
ros-jazzy-plansys2-executor/noble,now 2.0.18-1noble.20251026.062051 amd64 [installed]
ros-jazzy-plansys2-lifecycle-manager-dbgsym/noble,now 2.0.18-1noble.20251025.100334 amd64 [installed]
ros-jazzy-plansys2-lifecycle-manager/noble,now 2.0.18-1noble.20251025.100334 amd64 [installed]
ros-jazzy-plansys2-msgs-dbgsym/noble,now 2.0.18-1noble.20251025.084418 amd64 [installed]
ros-jazzy-plansys2-msgs/noble,now 2.0.18-1noble.20251025.084418 amd64 [installed]
ros-jazzy-plansys2-pddl-parser-dbgsym/noble,now 2.0.18-1noble.20251025.085245 amd64 [installed]
ros-jazzy-plansys2-pddl-parser/noble,now 2.0.18-1noble.20251025.085245 amd64 [installed]
ros-jazzy-plansys2-planner-dbgsym/noble,now 2.0.18-1noble.20251025.100644 amd64 [installed]
ros-jazzy-plansys2-planner/noble,now 2.0.18-1noble.20251025.100644 amd64 [installed]
ros-jazzy-plansys2-popf-plan-solver-dbgsym/noble,now 2.0.18-1noble.20251025.095632 amd64 [installed]
ros-jazzy-plansys2-popf-plan-solver/noble,now 2.0.18-1noble.20251025.095632 amd64 [installed]
ros-jazzy-plansys2-problem-expert-dbgsym/noble,now 2.0.18-1noble.20251025.100157 amd64 [installed]
ros-jazzy-plansys2-problem-expert/noble,now 2.0.18-1noble.20251025.100157 amd64 [installed]
ros-jazzy-plansys2-support-py/noble,now 2.0.18-1noble.20251025.085245 amd64 [installed]
ros-jazzy-plansys2-terminal-dbgsym/noble,now 2.0.18-1noble.20251026.063054 amd64 [installed]
ros-jazzy-plansys2-terminal/noble,now 2.0.18-1noble.20251026.063054 amd64 [installed]
ros-jazzy-plansys2-tests/noble,now 2.0.18-1noble.20251026.063343 amd64 [installed]
ros-jazzy-plansys2-tools-dbgsym/noble,now 2.0.18-1noble.20251025.100657 amd64 [installed]
ros-jazzy-plansys2-tools/noble,now 2.0.18-1noble.20251025.100657 amd64 [installed]

Domain:

(define (domain ass_domain)
    (:requirements :strips :typing :durative-actions :fluents)

    (:types
        robot
        point
        marker
    )

    (:predicates
        (robot_at ?r - robot ?p - point)
        (marker_at ?m - marker ?p - point)
        (is_next ?m1 ?m2 - marker)
        (is_first ?m - marker)
        (visited ?m - marker)
        (unvisited ?m - marker)
        (photo_taken ?m - marker)
        (photo_untaken ?m - marker)
        (robot_free ?r - robot)
        (detecting ?r - robot)
        (acquiring_imgs ?r - robot)
        (is_base ?p - point)

    )

    (:functions
        (state ?s - robot)
        (num_id_detected)
        (num_photo_taken)
        (num_marker)
        (marker_id ?m - marker)
    )

    (:durative-action detect_id
        :parameters (?r - robot ?p1 ?p2 - point ?m - marker)
        :duration ( = ?duration 5)
        :condition (and
            (at start(detecting ?r))
            (over all(marker_at ?m ?p1))
            (at start(unvisited ?m))
            (at start(robot_at ?r ?p2))
            (at start (robot_free ?r))
        )
        :effect (and
            (at start(not(robot_free ?r)))
            (at end(robot_free ?r))
            (at end(robot_at ?r ?p1))
            (at end(not(robot_at ?r ?p2)))
            (at end(not(unvisited ?m)))
            (at end(visited ?m))
            (at end(increase (num_id_detected) 1))
        )
    )
    
    (:durative-action change_to_acquire_state
        :parameters (?r - robot)
        :duration ( = ?duration 3)
        :condition (and 
            (at start(detecting ?r))
            (at start(robot_free ?r))
            (at start(= (num_id_detected) (num_marker)))
        )
        :effect(and
            (at end(not(detecting ?r)))
            (at end(acquiring_imgs ?r))
            (at start(not(robot_free ?r)))
            (at end(robot_free ?r))
        )
    )

    (:durative-action capture_first_img
        :parameters (?r - robot ?p1 ?p2 - point ?m1 - marker)
        :duration ( = ?duration 5)
        :condition (and
            (at start(photo_untaken ?m1))
            (at start(is_first ?m1))
            (at start(acquiring_imgs ?r))
            (over all(marker_at ?m1 ?p1))
            (at start(robot_at ?r ?p2))
            (at start(robot_free ?r))
        )
        :effect (and
            (at start(not(robot_free ?r)))
            (at end(robot_free ?r))
            (at end(robot_at ?r ?p1))
            (at end(not (robot_at ?r ?p2)))
            (at end(photo_taken ?m1))
            (at end(not(photo_untaken ?m1)))  
            (at end(increase (num_photo_taken) 1))  
        )
    )

    (:durative-action capture_other_imgs
        :parameters (?r - robot ?p1 ?p2 - point ?m1 ?m2 - marker)
        :duration ( = ?duration 5)
        :condition (and
            (at start(photo_untaken ?m1))
            (at start(is_next ?m1 ?m2))
            (at start(photo_taken ?m2))
            (at start(acquiring_imgs ?r))
            (over all(marker_at ?m1 ?p1))
            (at start(robot_at ?r ?p2))
            (at start(robot_free ?r))
        )
        :effect (and
            (at start(not(robot_free ?r)))
            (at end(robot_free ?r))
            (at end(robot_at ?r ?p1))
            (at end(not (robot_at ?r ?p2)))
            (at end(photo_taken ?m1))
            (at end(not(photo_untaken ?m1)))  
            (at end(increase (num_photo_taken) 1))  
        )
    )

    (:durative-action return_to_base
        :parameters (?r - robot ?base ?p - point)
        :duration (= ?duration 7)
        :condition (and 
            (at start(robot_free ?r))
            (at start(is_base ?base))
            (at start(robot_at ?r ?p))
            (at start(= (num_photo_taken) (num_marker)))
        )
        :effect (and 
            (at start(not(robot_free ?r)))
            (at end(robot_free ?r))
            (at end(robot_at ?r ?base))
            (at end(not(robot_at ?r ?p)))
        )
    )
)

Problem:

(define 
    (problem ass_problem)
    (:domain ass_domain)
    (:objects
        robot1 - robot
        base p1 p2 p3 p4 - point
        m1 m2 m3 m4 - marker
    )

    (:init
        (= (num_marker) 4)
        (robot_at robot1 base)
        (robot_free robot1)
        (detecting robot1)

        (= (num_id_detected) 0)
        (= (num_photo_taken) 0)

        (= (marker_id m1) -1)
        (= (marker_id m2) -1)
        (= (marker_id m3) -1)
        (= (marker_id m4) -1)

        (is_base base)

        (marker_at m1 p1)
        (marker_at m2 p2)
        (marker_at m3 p3)
        (marker_at m4 p4)

        (unvisited m1)
        (unvisited m2)
        (unvisited m3)
        (unvisited m4)

        (photo_untaken m1)
        (photo_untaken m2)
        (photo_untaken m3)
        (photo_untaken m4)

        (is_first m3)
        (is_next m4 m3)
        (is_next m1 m4)
        (is_next m2 m1)
    )

    (:goal
        ( and
            (photo_taken m1)            
            (photo_taken m2)      
            (photo_taken m3)      
            (photo_taken m4)  
            (robot_at robot1 base)    
        )
    )
)

Using the command:

 ros2 run popf popf ass_domain.pddl ass_problem.pddl

Plan obtained:

Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] [110%]
Post filtering unreachable actions:  [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] [110%]
None of the ground temporal actions in this problem have been recognised as compression-safe
b (9.000 | 28.005)b (8.000 | 28.005)b (6.000 | 33.006)b (5.000 | 38.007)b (4.000 | 38.007)b (3.000 | 43.008)b (2.000 | 43.008)b (1.000 | 50.009);;;; Solution Found
; Time 0.00
0.000: (detect_id robot1 p4 base m4)  [5.000]
5.001: (detect_id robot1 p1 p4 m1)  [5.000]
10.002: (detect_id robot1 p3 p1 m3)  [5.000]
15.003: (detect_id robot1 p2 p3 m2)  [5.000]
20.004: (change_to_acquire_state robot1)  [3.000]
23.005: (capture_first_img robot1 p3 p2 m3)  [5.000]
28.006: (capture_other_imgs robot1 p4 p3 m4 m3)  [5.000]
33.007: (capture_other_imgs robot1 p1 p4 m1 m4)  [5.000]
38.008: (capture_other_imgs robot1 p2 p1 m2 m1)  [5.000]
43.009: (return_to_base robot1 base p2)  [7.000]

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions