Unsolvable Plan [ The goal fact:(visited wp0) ]

[waiyc]

Hi,
I am currently using turtlebot3 with gazebo to test the ROSplan behavior.
I am trying to add 3 simulated actions [dock, undock and localize] on top of the implemented goto_waypoints action.
So the ideal generated plans in plan.pddl should look like:

0.000: (undock kenny wp0)  [10.000]
10.001: (localise kenny)  [60.000]
70.002: (goto_waypoint kenny wp0 wp1)  [60.000]
130.003: (goto_waypoint kenny wp1 wp0)  [60.000]
190.004: (dock kenny wp0)  [30.000]

However, I faced this error when I try to run ./turtlebot_explore.bash :

[ INFO] [1594914411.303336553, 11.710000000]: KCL: (/rosplan_planner_interface) (problem.pddl) Planning complete
[ INFO] [1594914411.303437455, 11.710000000]: KCL: (/rosplan_planner_interface) (problem.pddl) Plan was unsolvable.
[ INFO] [1594914412.343027039, 12.750000000]: KCL: (/rosplan_parsing_interface) No new planner output received; nothing to parse.
[ INFO] [1594914413.312519349, 13.720000000]: KCL: (/rosplan_plan_dispatcher) Dispatching plan.
[ INFO] [1594914413.405947330, 13.820000000]: KCL: (/rosplan_plan_dispatcher) Dispatch complete.

and the error message the generated in the plan.pddl :

Number of literals: 2
Constructing lookup tables:
A problem has been encountered, and the problem has been deemed unsolvable
--------------------------------------------------------------------------
The goal fact:
(visited wp0)

...cannot be found either in the initial state, as an add effect of an
 action, or as a timed initial literal.  As such, the problem has been deemed
unsolvable.

This is the content for ./turtlebot_explore.bash :

#!/bin/bash
rosservice call /rosplan_roadmap_server/create_prm "{nr_waypoints: 2, min_distance: 0.3, casting_distance: 2.0, connecting_distance: 8.0, occupancy_threshold: 10, total_attempts: 10}";

# call turtlebot_explore_common.bash script
bash `rospack find rosplan_interface_custom_demo`/scripts/turtlebot_explore_common.bash

This is the content for ./turtlebot_explore_common.bash :

#!/bin/bash

# add robot kenny instance + goals: visit all waypoint instances
echo "Adding initial state and goals to knowledge base.";
param_type="update_type:
- 0";
param="knowledge:
- knowledge_type: 0
  instance_type: 'robot'
  instance_name: 'kenny'
  attribute_name: ''
  function_value: 0.0";
param_type="$param_type
- 0";
param="$param
- knowledge_type: 1
  instance_type: ''
  instance_name: ''
  attribute_name: 'robot_at'
  values:
  - {key: 'r', value: 'kenny'}
  - {key: 'wp', value: 'wp0'}
  function_value: 0.0";
for i in $(rosservice call /rosplan_knowledge_base/state/instances "type_name: 'waypoint'" | grep -ohP "wp\d+")
do
param_type="$param_type
- 1"
param="$param
- knowledge_type: 1
  instance_type: ''
  instance_name: ''
  attribute_name: 'visited'
  values:
  - {key: 'wp', value: '$i'}
  function_value: 0.0"
done;

rosservice call /rosplan_knowledge_base/update_array "
$param_type
$param"

# NOTE: robot_at(kenny wp0) gets added by the mapping interface

# automatically generate PDDL problem from KB snapshot (e.g. fetch knowledge from KB and create problem.pddl)

echo "Calling problem generator.";
rosservice call /rosplan_problem_interface/problem_generation_server;

# make plan (e.g. call popf to create solution)
echo "Calling planner interface.";
rosservice call /rosplan_planner_interface/planning_server;

# parse plan (parse console output and extract actions and params, e.g. create esterel graph)
echo "Calling plan parser.";
rosservice call /rosplan_parsing_interface/parse_plan;

# dispatch (execute) plan. (send actions one by one to their respective interface and wait for them to finish)
echo "Calling plan dispatcher.";
rosservice call /rosplan_plan_dispatcher/dispatch_plan;

echo "Finished!";

Besides, this is the input problem.pddl:

(define (problem task)
(:domain turtlebot)
(:objects
    wp0 wp1 - waypoint
    kenny - robot
)
(:init
    (robot_at kenny wp0)
    (docked kenny)
    (dock_at wp0)
)
(:goal (and
    (visited wp0)
    (visited wp1)
    (docked kenny)
)))

and this is the domain.pddl:

(define (domain turtlebot)

(:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions)

(:types
	waypoint 
	robot
)

(:predicates
	(robot_at ?v - robot ?wp - waypoint)
	(visited ?wp - waypoint)
	(undocked ?v - robot)
	(docked ?v - robot)
	(localised ?v - robot)
	(dock_at ?wp - waypoint)
)

(:functions
    (charge ?r - robot)
)

;; Move to any waypoint, avoiding terrain
(:durative-action goto_waypoint
	:parameters (?v - robot ?from ?to - waypoint)
	:duration ( = ?duration 60)
	:condition (and
		(at start (robot_at ?v ?from))
		(at start (localised ?v))
		(over all (undocked ?v)))
	:effect (and
		(at end (visited ?to))
		(at end (robot_at ?v ?to))
		(at start (not (robot_at ?v ?from))))
)

;; Localise
(:durative-action localise
	:parameters (?v - robot)
	:duration ( = ?duration 60)
	:condition (over all (undocked ?v))
	:effect (at end (localised ?v))
)

;; Dock to charge
(:durative-action dock
	:parameters (?v - robot ?wp - waypoint)
	:duration ( = ?duration 30)
	:condition (and
		(over all (dock_at ?wp))
		(at start (robot_at ?v ?wp))
		(at start (undocked ?v)))
	:effect (and
		(at end (docked ?v))
		(at end (assign (charge ?v) 100))
		(at start (not (undocked ?v))))
)

(:durative-action undock
	:parameters (?v - robot ?wp - waypoint)
	:duration ( = ?duration 10)
	:condition (and
		(over all (dock_at ?wp))
		(at start (docked ?v)))
	:effect (and
		(at start (not (docked ?v)))
		(at end (undocked ?v)))
)

)

[waiyc]

However, a proper plan can be generated if I run simulated_actions.bash:

#!/bin/bash

rosservice call /rosplan_problem_interface/problem_generation_server;
rosservice call /rosplan_planner_interface/planning_server;
rosservice call /rosplan_parsing_interface/parse_plan;
rosservice call /rosplan_plan_dispatcher/dispatch_plan;

and this is the generated plan.pddl:

Number of literals: 7
Constructing lookup tables:
Post filtering unreachable actions: 
�[01;34mNo analytic limits found, not considering limit effects of goal-only operators�[00m
85% of the ground temporal actions in this problem are compression-safe
Initial heuristic = 4.000
b (3.000 | 130.002)b (2.000 | 160.003)
Resorting to best-first search
b (3.000 | 130.002)b (2.000 | 190.003)b (1.000 | 220.004);;;; Solution Found
; States evaluated: 23
; Cost: 220.004
; Time 0.00
0.000: (undock kenny wp0)  [10.000]
10.001: (localise kenny)  [60.000]
70.002: (goto_waypoint kenny wp0 wp1)  [60.000]
130.003: (goto_waypoint kenny wp1 wp0)  [60.000]
190.004: (dock kenny wp0)  [30.000]

This is case, undock and localize simulated action interface can be completed.
However, since no actual way-points were generated the RPMoveBase.cpp will encounter error

[ERROR] [1594916379.714855264, 46.270000000]: Waypoint not found in parameter server 

[Michael Cashmore]

Hello Waiyc,

I'm not sure what could be the error here - can you post some of the output log? It seems as though the turtlebot exploration script is somehow making the problem unsolvable. My guess would be that although the input problem is fine, after generating waypoints, the actual problem is unsolvable because (a) the "at robot" fact has been removed, or (b) not enough waypoints were generated to match the goal.

In any case, a quick solution would be to use simulated_actions.bash, and add a call to the waypoint generation from there - or load a fixed waypoints file if you prefer.

Michael