Plansys2_bt_example: Plan not found

[RicardoM17]

I'm following the instructions for this example on the documentation: Link

When I enter the commands in the plansys2 terminal I get a "Plan not found" error

> set instance r2d2 robot
> 
> set instance wheels_zone zone
> set instance steering_wheels_zone zone
> set instance body_car_zone zone
> set instance assembly_zone zone
> 
> set instance wheel_1 piece
> set instance body_car_1 piece
> set instance steering_wheel_1 piece
> 
> set instance wheel_2 piece
> set instance body_car_2 piece
> set instance steering_wheel_2 piece
> 
> set instance wheel_3 piece
> set instance body_car_3 piece
> set instance steering_wheel_3 piece
> 
> set instance car_1 car
> set instance car_2 car
> set instance car_3 car
> 
> set predicate (robot_at r2d2 wheels_zone)
> set predicate (is_assembly_zone assembly_zone)
> 
> set predicate (piece_at wheel_1 assembly_zone)
> set predicate (piece_at body_car_1 assembly_zone)
> set predicate (piece_at steering_wheel_1 assembly_zone)
> 
> set predicate (piece_is_wheel wheel_1)
> set predicate (piece_is_body_car body_car_1)
> set predicate (piece_is_steering_wheel steering_wheel_1)
> set predicate (piece_not_used wheel_1)
> set predicate (piece_not_used body_car_1)
> set predicate (piece_not_used steering_wheel_1)
> 
> set goal (and(car_assembled car_1))
> run
[ERROR] [1627584157.141321444] [planner_client]: /planner/get_plan: Plan not found
Plan could not be computed 

I'm using the fake sim for reference with Ubuntu 20.04 and ROS2 Galactic.

Any ideas @fmrico ?

[RicardoM17]

After investigating a bit I realized that these lines are missing:

set instance recharge_zone zone

set predicate (is_recharge_zone recharge_zone)
set predicate (robot_available r2d2)

I'm also not sure if it's by design for simplicity but all the pieces are already in the assembly zone at the start, making the plan rather simple.

That being said I'm now facing 2 different issues:

1. The plan is not optimal as it doesn't move directly to the recharge zone but goes first to the assembly zone:

> get plan
plan: 
0   (move r2d2 wheels_zone assembly_zone)   5
5.001   (move r2d2 assembly_zone recharge_zone) 5
10.002  (recharge r2d2 recharge_zone)   5
15.003  (move r2d2 recharge_zone assembly_zone) 5
20.004  (assemble r2d2 assembly_zone wheel_1 body_car_1 steering_wheel_1 car_1) 5

2. When running the actual plan it can't go to the second move action giving the following error:

[bt_action_node-2] [PublisherZMQ] Server quitting.
[bt_action_node-2] [PublisherZMQ] just died. Exeption Context was terminated
[bt_action_node-2] [PublisherZMQ] Publisher quitting.
[bt_action_node-2] [PublisherZMQ] just died. Exeption Context was terminated
[bt_action_node-2] [PublisherZMQ] just died. Exeption Socket operation on non-socket
[bt_action_node-2] [INFO] [1627635879.350604671] [move_1]: Waiting for "navigate_to_pose" action server
[bt_action_node-2] [INFO] [1627635879.350792166] [move_1]: "move" BtActionNode initialized
[bt_action_node-2] [ERROR] [1627635879.358654112] []: Caught exception in callback for transition 13
[bt_action_node-2] [ERROR] [1627635879.358664422] []: Original error: parameter 'waypoints' has already been declared
[bt_action_node-2] [WARN] [1627635879.358680232] []: Error occurred while doing error handling.

The first move actions succeeds however.

[fmrico]

Let me check...

[fmrico]

Hi,

1. The plan is not optimal as it doesn't move directly to the recharge zone but goes first to the assembly zone:

I am afraid that this problem is related to how the planner calculates the plan. Maybe with some extra parameters calling popf could fix it.

2. When running the actual plan it can't go to the second move action giving the following error:

Fixed in https://github.com/IntelligentRoboticsLabs/ros2_planning_system_examples/pull/25

Best

[RicardoM17]

1. The plan is not optimal as it doesn't move directly to the recharge zone but goes first to the assembly zone:

I am afraid that this problem is related to how the planner calculates the plan. Maybe with some extra parameters calling popf could fix it.

Could be but FYI using TFD the optimal plan is computed. All the info is there from the pddl POV but it seems to always want to assemble first without checking all the requirements.

When running the actual plan it can't go to the second move action giving the following error:

Fixed in #25

Correct! Tested on my end with both POPF and TFD and it now runs smoothly.

Thanks a lot for the help @fmrico. I'm closing this issue. if still want to investigate the non-optimal plan by POPF we can create a new issue instead but it's not really relevant for this repo.