I'm struggling to run a Task and Motion planning with PDDL and can't seems to find a solution. I guess it's me not doing the right thing but I'm not sure what.
yyaker@yyaker-virtual-machine:~/pyroborsim_ws$ ros2 run pyrobosim_ros entrepot_pddl_planner.py
[INFO] [1687934610.340197848] [entrepot_pddlstream_planner]: Planning node ready.
[INFO] [1687934610.340669487] [entrepot_pddlstream_planner]: Waiting for goal specification...
Received new goal specification!
[INFO] [1687934664.712488846] [entrepot_pddlstream_planner]: Requesting world state...
[INFO] [1687934664.738504206] [entrepot_pddlstream_planner]: Planning...
Iteration: 1 | Complexity: 0 | Skeletons: 0 | Skeleton Queue: 0 | Disabled: 0 | Evaluations: 58 | Eager Calls: 30 | Cost: inf | Search Time: 0.005 | Sample Time: 0.001 | Total Time: 0.007
Attempt: 1 | Results: 0 | Depth: 0 | Success: True | Time: 0.108
Stream plan (0, 0, 0.000): []
Action plan (0, 0.000): []
Summary: {complexity: 0, cost: 0.000, evaluations: 58, iterations: 1, length: 2, run_time: 0.116, sample_time: 0.001, search_time: 0.114, skeletons: 1, solutions: 1, solved: True, timeout: False}
Iteration: 1 | Complexity: 0 | Skeletons: 0 | Skeleton Queue: 0 | Disabled: 0 | Evaluations: 58 | Eager Calls: 30 | Cost: inf | Search Time: 0.006 | Sample Time: 0.002 | Total Time: 0.008
Attempt: 1 | Results: 0 | Depth: 0 | Success: True | Time: 0.063
Stream plan (0, 0, 0.000): []
Action plan (0, 0.000): []
Summary: {complexity: 0, cost: 0.000, evaluations: 58, iterations: 1, length: 2, run_time: 0.071, sample_time: 0.002, search_time: 0.070, skeletons: 1, solutions: 1, solved: True, timeout: False}
Iteration: 1 | Complexity: 0 | Skeletons: 0 | Skeleton Queue: 0 | Disabled: 0 | Evaluations: 58 | Eager Calls: 30 | Cost: inf | Search Time: 0.006 | Sample Time: 0.002 | Total Time: 0.008
Attempt: 1 | Results: 0 | Depth: 0 | Success: True | Time: 0.033
Stream plan (0, 0, 0.000): []
Action plan (0, 0.000): []
Summary: {complexity: 0, cost: 0.000, evaluations: 58, iterations: 1, length: 2, run_time: 0.041, sample_time: 0.002, search_time: 0.040, skeletons: 1, solutions: 1, solved: True, timeout: False}
[INFO] [1687934664.999101148] [entrepot_pddlstream_planner]: None
[INFO] [1687934664.999636181] [entrepot_pddlstream_planner]: Robot: lrobot
[INFO] [1687934665.000054886] [entrepot_pddlstream_planner]: [('At', Robot: lrobot, Room: warehouse), ('At', Object: box_A0, Object spawn: table0_tabletop)]
Traceback (most recent call last):
File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/lib/pyrobosim_ros/entrepot_pddl_planner.py", line 174, in <module>
main()
File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/lib/pyrobosim_ros/entrepot_pddl_planner.py", line 167, in main
rclpy.spin(planner_node)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 222, in spin
executor.spin_once()
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 713, in spin_once
raise handler.exception()
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 239, in __call__
self._handler.send(None)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 418, in handler
await call_coroutine(entity, arg)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 332, in _execute_timer
await await_or_execute(tmr.callback)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute
return callback(*args)
File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/lib/pyrobosim_ros/entrepot_pddl_planner.py", line 109, in timer_callback
self.do_plan()
File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/lib/pyrobosim_ros/entrepot_pddl_planner.py", line 157, in do_plan
plan_msg = task_plan_to_ros(plan)
File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim_ros/ros_conversions.py", line 208, in task_plan_to_ros
raise Exception("Input is not a TaskPlan object")
Exception: Input is not a TaskPlan object
[ros2run]: Process exited with failure 1
`#!/usr/bin/env python3
"""
Test script showing how to use a PDDLStream planner as a ROS 2 node.
"""
import os
import time
import rclpy
from rclpy.node import Node
from pyrobosim.core import WorldYamlLoader
from pyrobosim.planning import PDDLStreamPlanner
from pyrobosim.planning.pddlstream.utils import get_default_domains_folder
from pyrobosim.utils.general import get_data_folder
from pyrobosim_ros.ros_interface import update_world_from_state_msg
from pyrobosim_ros.ros_conversions import (
goal_specification_from_ros,
task_plan_to_ros,
)
from pyrobosim_msgs.msg import GoalSpecification, TaskPlan
from pyrobosim_msgs.srv import RequestWorldState
def load_world():
"""Load a test world."""
loader = WorldYamlLoader()
world_file = "entrepot_world.yaml"
data_folder = get_data_folder()
return loader.from_yaml(os.path.join(data_folder, world_file))
class PlannerNode(Node):
def __init__(self):
self.latest_goal = None
self.planning = False
super().__init__("entrepot_pddlstream_planner")
# Declare parameters
self.declare_parameter("example", value="01_simple")
self.declare_parameter("subscribe", value=True)
self.declare_parameter("verbose", value=True)
self.declare_parameter("search_sample_ratio", value=0.5)
# Publisher for a task plan
self.plan_pub = self.create_publisher(TaskPlan, "commanded_plan", 10)
# Service client for world state
self.world_state_client = self.create_client(
RequestWorldState, "request_world_state"
)
self.world_state_future_response = None
while rclpy.ok() and not self.world_state_client.wait_for_service(
timeout_sec=1.0
):
self.get_logger().info("Waiting for world state server...")
if not self.world_state_client.wait_for_service(timeout_sec=1.0):
self.get_logger().error("Error while waiting for the world state server.")
return
# Create the world and planner
self.world = load_world()
example = self.get_parameter("example").value
domain_folder = os.path.join(get_default_domains_folder(), example)
self.planner = PDDLStreamPlanner(self.world, domain_folder)
self.get_logger().info("Planning node ready.")
if self.get_parameter("subscribe").value == True:
self.get_logger().info("Waiting for goal specification...")
# Subscriber to task plan
self.goalspec_sub = self.create_subscription(
GoalSpecification, "goal_specification", self.goalspec_callback, 10
)
else:
if example == "01_simple":
# Task specification for simple example.
self.latest_goal = [
("At", "lrobot", "warehouse"),
("At", "box_A0", "table0_tabletop"),
]
elif example in [
"02_derived",
"03_nav_stream",
"04_nav_manip_stream",
"05_nav_grasp_stream",
]:
# Task specification for derived predicate example.
self.latest_goal = [
("Has", "desk0_desktop", "banana0"),
("Has", "counter", "apple1"),
("HasNone", "bathroom", "banana"),
("HasAll", "table", "water"),
]
else:
print(f"Invalid example: {example}")
return
time.sleep(2.0)
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
if (not self.planning) and self.latest_goal:
self.request_world_state()
if self.world_state_future_response and self.world_state_future_response.done():
self.do_plan()
def request_world_state(self):
"""Requests a world state from the world."""
self.planning = True
self.get_logger().info("Requesting world state...")
self.world_state_future_response = self.world_state_client.call_async(
RequestWorldState.Request()
)
def goalspec_callback(self, msg):
"""
Handle goal specification callback.
:param msg: Goal specification message to process.
:type msg: :class:`pyrobosim_msgs.msg.TaskPlan`
"""
print("Received new goal specification!")
self.latest_goal = goal_specification_from_ros(msg, self.world)
def do_plan(self):
"""Search for a plan and publish it."""
if not self.latest_goal:
return
# Unpack the latest world state.
try:
result = self.world_state_future_response.result()
update_world_from_state_msg(self.world, result.state)
except Exception as e:
self.get_logger().info("Failed to unpack world state.")
# Once the world state is set, plan using the first robot.
self.get_logger().info("Planning...")
robot = self.world.robots[0]
plan = self.planner.plan(
robot,
self.latest_goal,
max_attempts=3,
search_sample_ratio=self.get_parameter("search_sample_ratio").value,
planner="ff-astar",
max_planner_time=10.0,
max_time=60.0
)
if self.get_parameter("verbose").value == True:
self.get_logger().info(f"{plan}")
self.get_logger().info(f"{robot}")
self.get_logger().info(f"{self.latest_goal}")
plan_msg = task_plan_to_ros(plan)
self.plan_pub.publish(plan_msg)
self.latest_goal = None
self.planning = False
def main():
rclpy.init()
planner_node = PlannerNode()
rclpy.spin(planner_node)
planner_node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
`#!/usr/bin/env python3
"""
Test script showing how to publish a goal specification to a PDDLStream planner node.
"""
import rclpy
from rclpy.node import Node
import time
from pyrobosim_msgs.msg import GoalPredicate, GoalSpecification
class GoalPublisher(Node):
def __init__(self):
super().__init__("entrepot_pddlstream_goal_publisher")
# Declare parameters
self.declare_parameter("example", value="01_simple")
self.declare_parameter("verbose", value=True)
# Publisher for a goal specification
self.goalspec_pub = self.create_publisher(
GoalSpecification, "goal_specification", 10
)
self.get_logger().info("Waiting for subscription")
while rclpy.ok() and self.goalspec_pub.get_subscription_count() < 1:
time.sleep(2.0)
if self.goalspec_pub.get_subscription_count() < 1:
self.get_logger().error(
"Error while waiting for a goal specification subscriber."
)
return
# Create goal specifications for different examples
example = self.get_parameter("example").value
if example == "01_simple":
# Goal specification for simple example.
goal_predicates = [
GoalPredicate(type="At", args=("lrobot", "warehouse")),
GoalPredicate(type="At", args=("box_A0", "table0_tabletop")),
]
elif example in [
"02_derived",
"03_nav_stream",
"04_nav_manip_stream",
"05_nav_grasp_stream",
]:
# Goal specification for derived predicate example.
goal_predicates = [
GoalPredicate(type="Has", args=("desk0_desktop", "banana0")),
GoalPredicate(type="Has", args=("counter", "apple1")),
GoalPredicate(type="HasNone", args=("bathroom", "banana")),
GoalPredicate(type="HasAll", args=("table", "water")),
]
else:
self.get_logger().info(f"Invalid example: {example}")
return
# Publish and optionally display the goal specification message.
goal_msg = GoalSpecification(predicates=goal_predicates)
if self.get_parameter("verbose").value == True:
msg = "Published goal specification:"
for pred in goal_msg.predicates:
msg += f"\n{pred}"
self.get_logger().info(msg)
self.goalspec_pub.publish(goal_msg)
def main():
rclpy.init()
goal_node = GoalPublisher()
rclpy.spin(goal_node)
goal_node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()