ROS 2 enabled 2D mobile robot simulator for behavior prototyping.
Home Page: https://pyrobosim.readthedocs.io/
License: MIT License
ROS 2 enabled 2D mobile robot simulator for behavior prototyping.
By Sebastian Castro, 2022-2024
Refer to the full documentation for setup, usage, and other concepts.
We look forward to your open-source contributions to pyrobosim. For more information, refer to the contributor guide.
This will be nice to have generally, and is also a requirement to submitting to JOSS (... you know, if I ever feel inspired)
Right now, all the "Pick" actions in pyrobosim assume the robot can perfectly find a grasp and motion plan to an object.
Consider adding basic grasp generation on cuboid objects (plus a way to fit cuboids to more complex objects), so that robots can now fail to plan and/or execute picking of objects in a world.
This should also connect to the task and motion planning tooling with PDDLStream, where grasp sampling can be a stream in a planning domain.
Most ROS 2 wrapper packages do this.
There should be a pyrobosim package that has the Python only implementation, and a pyrobosim_ros package with the ROS specific code such as nodes, launch files, message definitions, etc.
If you just open python in a console where pyrobosim is set up you can do this
(pyrobosim) henry@henry-ubuntu:~/picknik/pyrobosim (world_knowledge_tests)$ python
Python 3.8.10 (default, May 26 2023, 14:05:08)
[GCC 9.4.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> from pyrobosim.utils.knowledge import resolve_to_location
Traceback (most recent call last):
File "<stdin>", line 1, in <module>
File "/home/henry/.local/lib/python3.8/site-packages/pyrobosim/utils/knowledge.py", line 10, in <module>
from ..core.locations import Location, ObjectSpawn
File "/home/henry/.local/lib/python3.8/site-packages/pyrobosim/core/__init__.py", line 14, in <module>
from .robot import *
File "/home/henry/.local/lib/python3.8/site-packages/pyrobosim/core/robot.py", line 11, in <module>
from ..utils.knowledge import resolve_to_object
ImportError: cannot import name 'resolve_to_object' from partially initialized module 'pyrobosim.utils.knowledge' (most likely due to a circular import) (/home/henry/.local/lib/python3.8/site-packages/pyrobosim/utils/knowledge.py)
You can get around it by just importing pyrobosim.core first:
(pyrobosim) henry@henry-ubuntu:~/picknik/pyrobosim (world_knowledge_tests)$ python
Python 3.8.10 (default, May 26 2023, 14:05:08)
[GCC 9.4.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import pyrobosim.core
>>> from pyrobosim.utils.knowledge import resolve_to_location
>>>
from ..core.locations import Location, ObjectSpawn imports core as a whole, and robot.py then has from ..utils.knowledge import resolve_to_object
Not sure the best way to deal with it
Hello,
I get this error when I try to run the command "ros2 run pyrobosim_ros demo_pddl_planner.py --ros-args -p example:=01_simple -p subscribe:=false" :
Traceback (most recent call last): File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/lib/pyrobosim_ros/demo_pddl_planner.py", line 13, in <module> from pyrobosim.planning import PDDLStreamPlanner ImportError: cannot import name 'PDDLStreamPlanner' from 'pyrobosim.planning' (/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim/planning/__init__.py) [ros2run]: Process exited with failure 1
Can you help me ? I can't find a solution
Thanks
The world knowledge utilities, found at pyrobosim/pyrobosim/utils/knowledge.py, could use some unit testing.
These utilities perform several fundamental tasks used throughout pyrobosim, particularly for locating world entities (rooms, locations, objects, robot, etc.) given a world object and its corresponding location/object metadata.
These functions can be quite complex in that they need to convert generic queries (e.g., "kitchen apple") into specific instances, or lack thereof. As such, getting this functionality right has significant impact on the rest of pyrobosim.
The World class, found in pyrobosim/pyrobosim/core/world.py, needs more unit tests.
Right now, there are tests for adding/removing entities in the test/world_model folder.
However, some of the other methods around occupancy and graphs are not yet covered.
pyrobosim has its own occupancy grid implementation found at pyrobosim/pyrobosim/navigation/occupancy_grid.py.
This is an important tool for certain types of planners inside pyrobosim that use occupancy grids, and also for exporting grids for Gazebo world generation so that the worlds can be used with standard stacks such as Nav2.
When testing, pay attention to:
By creating a Docker image, users can easily set up a container on their machines to get started with pyrobosim and play with the demos without having to do the ROS2 / Python installs themselves.
NOTE: This is partially in place with the docker/ folder containing Ubuntu 20.04/ROS2 Foxy and Ubuntu 22.04/ROS2 Humble images, but right now they are used only for CI and the visualization likely doesn't work.
An __init__.py makes a directory into a python package.
The imports from individual modules should be added to __init__.py so that they can be used from the package namespace.
eg : from pyrobosim.navigation import OccupancyGrid rather than from pyrobosim.navigation.occupancy_grid import OccupancyGrid
Some reference on packages : https://docs.python.org/3/tutorial/modules.html#packages
Note : This should probably be clubbed in with #78
Once #1 is solved, we will have a set of planners that work directly with our polygon-based representation... which is expensive.
Given that we also have a way to generate an occupancy grid from world, we should have planning capabilities that leverage this. The ideas I have so far are:
Some good ideas might be to look into Peter Corke's Robotics Toolbox for Python and/or Atsushi Sakai's PythonRobotics package. I would be thrilled to have either or both as dependencies for the planners.
The Robot class, found in pyrobosim/pyrobosim/pyrobosim/core/robot.py, needs unit tests.
Pay specific attention to:
The current automated tests take a lot of time to run.
This could be improved by using docker images which are built weekly (as suggested by @sea-bass ) and also adding a manually triggered workflow to rebuild a docker image in case of a requirement to rebuild between the automated build interval.
EDIT :
It might also be useful to make the tests run based on dependencies.
For example,
The python tests only run if format tests pass
The ROS tests only run once the python tests pass.
The pyrobosim_ros package needs some unit tests, but these should be separate from the core pyrobosim tests since the ROS interface is optional.
As a proof of concept, we should add unit tests for the pyrobosim_ros/pyrobosim_ros/ros_conversions.py file and configure them to run with colcon test on that package.
The WorldYamlLoader class, found in pyrobosim/pyrobosim/core/yaml_utils.py, needs unit tests.
These tests should verify both valid and invalid YAML file loading, and correspond that the created World object has the right expected parameters based on the contents of the YAML files provided as input.
It would be nice to document these conventions somewhere, maybe at https://pyrobosim.readthedocs.io/en/latest/index.html
Quaternion order (qw, qx, qy, qz)
Euler angle convention (extrinsic x-y-z)
Rotations in radian
Reference frame. Is it world, odom, or map? I think it must be map in ROS, per this REP: https://www.ros.org/reps/rep-0105.html
I believe I've followed the instructions here to the letter but get this error. I am not using the Docker image nor ROS2. Just standalone. Ubuntu 22.
[andy-G7-7500][main] pyrobosim$ python3 examples/demo_pddl.py --example 01_simple verbose
Traceback (most recent call last):
File "/home/andy/ws_tp/pyrobosim/pyrobosim/examples/demo_pddl.py", line 13, in <module>
from pyrobosim.planning.pddlstream.planner import PDDLStreamPlanner
File "/home/andy/.local/lib/python3.10/site-packages/pyrobosim/planning/pddlstream/planner.py", line 4, in <module>
from pddlstream.algorithms.focused import solve_focused
ModuleNotFoundError: No module named 'pddlstream'
I've definitely already run this: ./setup/setup_pddlstream.bash and I see that solve_focused is defined here:
dependencies/pddlstream/pddlstream/algorithms/focused.py:def solve_focused_original(problem, fail_fast=False, **kwargs):
Cool blog and package btw!
Instead of having a lone Dockerfile and a bunch of shell scripts, switch to using a docker compose file which makes things much cleaner.
I just noticed this, but #82 actually removed some old functionality in pyrobosim that is no longer available.
This has to do with having a fully-connected roadmap graph with just the nodes included in the rooms / locations and directly doing A* search along this roadmap:
@ibrahiminfinite Could you confirm this is the case? Do you think you'd be able to restore this functionality by adding another planner that uses this directly? Right now, we only have A*
Several utilities and tests in pyrobosim rely on checking if poses are equal to each other.
Consider adding both
__eq__() method)Right now, the PDDLStreamPlanner object always uses function mappings defined in pyrobosim/pyrobosim/planning/pddlstream/mappings.py.
For users to define their own domain-specific mappings, instead of modifying this existing file in the pyrobosim repo, they should be able to specify their own file.
Ideally this mappings.py file could be stored in the same location as the domain.pddl and streams.pddl, so users can simply pass in the folder.
This issue addresses the need to implement standard sampling-based planners, namely RRT and PRM.
RRT will be particularly interesting in that it will define a difference between a global multi-query planner (like the existing search graph + the upcoming PRM), and a per-robot, single-query planner such as RRT.
RRT will also be helpful in later integration of task and motion planning (TAMP) as a way to sample instances of motion plans from a start to a goal pose.
I get this on main branch as well as PR #32 when running:
python3 examples/demo_pddl.py --example 01_simple --verbose
The polygon utilities, found under pyrobosim/pyrobosim/utils/polygon.py, could use more unit tests.
There already is a test file under test/utils/test_polygon_utils.py, but it's only testing one function at the moment.
There are lots of TODOs scattered throughout the code for how things should change when multiple robots exist.
Using this issue to track the multi-robot enhancement to pyrobosim.
Hi,
I followed your documentation and ran the following line:
ros2 run pyrobosim_ros demo.py
I saw a nice map. I clicked on Navigate and received the following error:
~/pyrobosim/pyrobosim$ ros2 run pyrobosim_ros demo.py [INFO] [1664361758.672906297] [pyrobosim]: World node started. [INFO] [1664361758.673251481] [pyrobosim]: Creating demo world programmatically. Navigating to desk0_desktop Exception in thread Thread-3: Traceback (most recent call last): File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner self.run() File "/usr/lib/python3.8/threading.py", line 870, in run self._target(*self._args, **self._kwargs) File "/home/gabriele-plankton/pyrobosim/install/pyrobosim_ros/lib/python3.8/site-packages/pyrobosim/navigation/execution.py", line 56, in execute (traj_t, traj_x, traj_y, traj_yaw) = interpolate_trajectory(traj, self.dt) File "/home/gabriele-plankton/pyrobosim/install/pyrobosim_ros/lib/python3.8/site-packages/pyrobosim/navigation/trajectory.py", line 97, in interpolate_trajectory [slerp(t).as_euler("xyz", degrees=False)[2] for t in t_interp] File "/home/gabriele-plankton/pyrobosim/install/pyrobosim_ros/lib/python3.8/site-packages/pyrobosim/navigation/trajectory.py", line 97, in <listcomp> [slerp(t).as_euler("xyz", degrees=False)[2] for t in t_interp] File "/usr/lib/python3/dist-packages/scipy/spatial/transform/rotation.py", line 1757, in __call__ raise ValueError("Expected times to be specified in a 1 " ValueError: Expected times to be specified in a 1 dimensional array, got 0 dimensions.
Hello,
I get this error when I try to launch the command "ros2 launch pyrobosim_ros demo_commands_multirobot.launch.py" :
[INFO] [launch]: All log files can be found below /home/yyaker/.ros/log/2023-07-03-11-39-28-542416-yyaker-virtual-machine-13463
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [demo.py-1]: process started with pid [13464]
[INFO] [demo_commands.py-2]: process started with pid [13466]
[demo.py-1] [INFO] [1688377170.511551120] [pyrobosim.demo_world]: World node started.
[demo.py-1] [INFO] [1688377170.512725045] [pyrobosim.demo_world]: Using world file test_world_multirobot.yaml.
[demo_commands.py-2] [INFO] [1688377171.094430860] [pyrobosim.demo_commands]: Publishing sample multirobot task plan...
[demo.py-1] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[demo.py-1] [robot1] Executing task plan...
[demo.py-1] [robot1] Executing action navigate [1/5]
[demo.py-1] Exception in thread Thread-5 (execute_plan):
[demo.py-1] Traceback (most recent call last):
[demo.py-1] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
[demo.py-1] self.run()
[demo.py-1] File "/usr/lib/python3.10/threading.py", line 953, in run
[demo.py-1] self._target(*self._args, **self._kwargs)
[demo.py-1] File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim/core/robot.py", line 429, in execute_plan
[demo.py-1] success = self.execute_action(act_msg, blocking=blocking)
[demo.py-1] File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim/core/robot.py", line 367, in execute_action
[demo.py-1] path = self.world.find_path(action.target_location, robot=self)
[demo.py-1] AttributeError: 'World' object has no attribute 'find_path'
[demo.py-1] [robot2] Executing task plan...
[demo.py-1] [robot2] Executing action navigate [1/4]
[demo.py-1] Exception in thread Thread-6 (execute_plan):
[demo.py-1] Traceback (most recent call last):
[demo.py-1] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
[demo.py-1] self.run()
[demo.py-1] File "/usr/lib/python3.10/threading.py", line 953, in run
[demo.py-1] self._target(*self._args, **self._kwargs)
[demo.py-1] File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim/core/robot.py", line 429, in execute_plan
[demo.py-1] success = self.execute_action(act_msg, blocking=blocking)
[demo.py-1] File "/home/yyaker/pyroborsim_ws/install/pyrobosim_ros/local/lib/python3.10/dist-packages/pyrobosim/core/robot.py", line 367, in execute_action
[demo.py-1] path = self.world.find_path(action.target_location, robot=self)
[demo.py-1] AttributeError: 'World' object has no attribute 'find_path'I looked at world.py file and it is true that there is no find_path attribute.
A question I ran across as a I started to work with this. From the 04_nav_manip_stream demo, why do some of the parameters in the goal specification include get(), others don't?
goal_literals = [ ("Has", get("desk0_desktop"), get("banana0")), ("Has", "counter", get("apple1")), ("HasNone", get("bathroom"), "banana"), ("HasAll", "table", "water"), ]
counter, banana, table, water do not have get()
I'm guessing it is a way to specify all parameters of a certain type. Like, banana means all bananas but banana0 is the specific banana. Can that be done with any parameter?
Right now there is only minimal info available about the events that happen like a robot or planner being created.
This can be improved by using something like https://docs.python.org/3/library/logging.html so that we can display more information as necessary in order to get better observability and help with debugging.
It looks like navigate/pick/place are duplicated in all of the domain-defining pddl files. Prob could move those to a common data file.
https://github.com/sea-bass/pyrobosim/tree/main/pyrobosim/pyrobosim/data/pddlstream/domains
The file pyrobosim/pyrobosim/utils/motion.py contains utilities to convert a "raw" list of waypoints to something that can be passed into a trajectory generator/executor.
The utilities here perform important bridging tasks such as
Path class for representing pathsPath.fill_yaws())The file pyrobosim/pyrobosim/navigation/trajectory.py has a few utilities that help calculate trajectories from paths -- that is, they add timing information to a list of waypoints for a robot to follow.
These are:
get_constant_speed_trajectory to convert waypoints to a trajectory, under linear and (optionally) angular velocity limitsinterpolate_trajectory to take a sparse trajectory to something evenly sampled at a particular timeIn doing this, consider paying special attention to edge cases and invalid configurations that should gracefully error.
Right now, if you remove a room or location from the world, it will not delete any children.
Removing a location should probably remove all attached objects, and removing a room should probably remove all attached locations and objects of those locations.
The TaskAction and TaskPlan objects, which are used to represent actions (or sequences thereof) for task planning and execution, need unit tests.
These can be found in pyrobosim/pyrobosim/planning/actions.py.
As these are relatively simple objects with not much state, the tests should focus mostly on constructing objects and checking that the contents of the objects have the expected values based on the input arguments.
In #41, we had to pin the Shapely version to 1.8 because the 2.0 release broke creating the polygon patches with Descartes.
Since Descartes doesn't seem to be actively maintained, I propose upgrading to Shapely 2.0 and either:
matplotlib.patches.PathPatch as Descartes does internally. See e.g. this fork of Descartes for inspiration on how to implement the functionality.The msg has this field, the internal class does not. I guess the easy way to reconcile the two is to delete has_pose from TaskAction.msg.
I found 2 annoying things about the RRT implementation in testing other stuff:
We don't check if we can immediately connect to the goal over short distances. So some trees will look silly like this:
Furthermore, in a bidirectional RRT* implementation we don't try rewire the tree once we connect the start and end nodes. You can see that in the kink here, for example:
This second one might be a little trickier to do in that in may actually require the solution path to be added to the start tree and then running a rewire on all the newly connected nodes.
Having releases of pyrobosim that can be pip installed would be amazing.
The Pose class, found at pyrobosim/pyrobosim/utils/pose.py, is used all throughout pyrobosim and is in need of extensive unit tests.
When writing tests, pay close attention to:
Right now, there are a lot of duplicate arguments when calling World methods like add_room(), add_hallway(), etc.
Also, add_room() is the only one that takes in an object instead of arguments.
There is opportunity to unify all this by passing in a dictionary of keyword arguments (kwargs) instead.
pyrobosim has a SearchGraph class, found in pyrobosim/pyrobosim/utils/search_graph.py, which provides a graph representation for searching within a world.
This needs unit tests that focus on:
Current tests only cover a small portion of the functionalities.
More tests need to be added and also should set it up such that tests with uninstalled dependency are skipped.
A list of tests to be added will be update here once #82 is completed.
The current way of implementing and using the path planners are not very flexible and presents some complexity when they need to be extended to support more functionality.
A new planner interface is needed that will make implementing the planners to be used with pyrobosim easier.
This is currently being worked on at : new_planner_interface
Looking at the blog here: https://roboticseabass.com/2022/07/30/integrated-task-and-motion-planning-in-robotics/
It would be nice if there were an explanation of static vs fluent.
Now that gazebosim/gz-sim#1456 is merged, we should be able to generate worlds compatible with Ignition Fortress.
If we can prove this out, the goal would be to
pyrobosim/pyrobosim/core/gazebo.py:WorldGazeboExporter that configured whether we want to export to Gazebo classic or Ignition.Similar to this PR: #66
Probably should use pre-commit to add a config file and then also set up a github action for it.
A big reason for this repo existing is that it can be used to prototype task and motion planners.
One such planner which will be used as the main demo is PDDLStream (https://github.com/caelan/pddlstream).
We should integrate a few demos that use this planner as a regular PDDL planner (with only discrete variables), plus with the addition of continuous variables such as sampling placement poses and navigation plans (e.g., RRTs).
We will also have to figure out the necessary steps / instructions / scrips to ensure that PDDLStream works easily with this package.
Hello,
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.
Here is the error I get :
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
Here is my planner file :
`#!/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()Here is my goal_publisher file :
`#!/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()And finally, my world file :
`#!/usr/bin/env python3
"""
Test script showing how to start a pyrobosim world that receives a plan from a
Task and Motion Planner such as PDDLStream.
"""
import os
import rclpy
import threading
from pyrobosim.core import WorldYamlLoader
from pyrobosim.gui import start_gui
from pyrobosim.utils.general import get_data_folder
from pyrobosim_ros.ros_interface import WorldROSWrapper
def load_world():
"""Load a test world."""
world_file = os.path.join(get_data_folder(), "entrepot_world.yaml")
return WorldYamlLoader().from_yaml(world_file)
def create_ros_node():
"""Initializes ROS node"""
rclpy.init()
world = load_world()
node = WorldROSWrapper(world=world, name="pddl_demo", state_pub_rate=0.1)
return node
if __name__ == "__main__":
node = create_ros_node()
# Start ROS Node in separate thread
ros_thread = threading.Thread(target=node.start)
ros_thread.start()
# Start GUI in main thread
start_gui(node.world)Partially fixed by #63 but there are still some other while loops which I believe would hang forever when Ctrl+C is pressed. Here's an example:
pyrobosim/gui/world_canvas.py
while True:
# Check if any robot is currently navigating.
nav_status = [robot.executing_nav for robot in self.world.robots]
...
time.sleep(sleep_time)
Here's a simple solution: https://stackoverflow.com/a/18994932
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
Django
The Web framework for perfectionists with deadlines.
Laravel
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
Microsoft
Open source projects and samples from Microsoft.
Google
Google ❤️ Open Source for everyone.
Alibaba
Alibaba Open Source for everyone
D3
Data-Driven Documents codes.
Tencent
China tencent open source team.