feat(worlds): add configurable simulation worlds

.gitignore

@@ -8,3 +8,4 @@
 .rover
 .docker_home/
 .ros/
+__pycache__/

CONTRIBUTING.md

@@ -52,7 +52,7 @@ the commit messages in the branch (recommended, not checked).
   | `revert` | revert of a previous change |
 
 - **scope** (optional) — the part of the project, lower case: a package without the `kpi_rover_` prefix
-  (`hw`, `description`, `bringup`, `localization`, `slam`), `nav`, `sim`, `docker`, `rover` (rover.sh),
+  (`hw`, `description`, `bringup`, `localization`, `slam`, `worlds`), `nav`, `sim`, `docker`, `rover` (rover.sh),
   `rpi-setup`, or the name of an application in `src/apps/`.
 - **description** — in English, imperative mood, lower case first letter, no period at the end:
   "add", not "added" or "adds".

README.md

@@ -61,6 +61,7 @@ maintainers and runs on every robot after boot.
 | `kpi_rover_localization` | EKF (`robot_localization`): fuses wheel odometry and IMU |
 | `kpi_rover_slam` | `slam_toolbox`: builds the map from the lidar and localizes the robot on it |
 | `kpi_rover_bringup` | launch files and configs: `robot.launch.py` (real robot), `sim.launch.py` (Gazebo), Nav2, lidar, joystick |
+| `kpi_rover_worlds` | Gazebo worlds for the simulation, configurable ones with their arguments, start points and previews. Not installed on the robot |
 
 **`src/apps/`** is for applications. An application does not drive motors or read sensors directly: it uses
 the platform topics, actions and services (`/cmd_vel`, Nav2, `/scan`, `/map`, ...), so the same application
@@ -159,6 +160,21 @@ source install/setup.bash
 ros2 launch kpi_rover_bringup sim.launch.py
 ```
 
+**Worlds.** `world:=` takes a world of `kpi_rover_worlds` by name (`room` by default) or a path to a world file,
+`start:=` puts the robot at one of the start points of the world instead of `x_pose`/`y_pose`. A configurable world
+(`garden`, `maze`, `overpass`, `test_square`) given by name has its default arguments; with other ones, build it first:
+```bash
+python3 -m kpi_rover_worlds.world_info          # worlds, their arguments and defaults
+ros2 launch kpi_rover_bringup sim.launch.py world:=garden start:=road_a
+ros2 launch kpi_rover_bringup sim.launch.py start:=a \
+  world:=$(python3 -m kpi_rover_worlds.world_info --build test_square floor:=asphalt marking:=yellow)
+```
+A world is a directory `worlds/<name>/` of `kpi_rover_worlds`: the world file, its manifest `<name>.yaml` and
+its previews.
+`sim.launch.py` is `world.launch.py` (Gazebo with the world) and `sim_robot.launch.py` (the simulated robot with the
+same localization, SLAM and navigation as the real one) together, with RViz and joystick teleop; tools such as the
+GUI start them one by one.
+
 Keyboard control (second zellij pane):
 ```bash
 ros2 run teleop_twist_keyboard teleop_twist_keyboard \

docker/Dockerfile

@@ -71,9 +71,12 @@ CMD ["zellij"]
 FROM ros:${ROS_DISTRO}-ros-base AS build-deps
 ARG SIM_ONLY_KEYS
 COPY --from=manifests /src /tmp/src
-RUN apt-get update && \
+# kpi_rover_worlds is built, but its tools (world generation, previews) are not run on the robot:
+# their dependencies (numpy, scipy, PIL, Gazebo) are not installed.
+RUN rm -rf /tmp/src/platform/kpi_rover_worlds && \
+    apt-get update && \
     rosdep update --rosdistro "$ROS_DISTRO" && \
-    rosdep install --from-paths /tmp/src --ignore-src -y --skip-keys "$SIM_ONLY_KEYS" && \
+    rosdep install --from-paths /tmp/src --ignore-src -y --skip-keys "$SIM_ONLY_KEYS kpi_rover_worlds" && \
     rm -rf /var/lib/apt/lists/* /tmp/src
 
 

src/platform/kpi_rover_bringup/CMakeLists.txt

@@ -16,7 +16,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY launch config rviz worlds
+  DIRECTORY launch config rviz
   DESTINATION share/${PROJECT_NAME}
 )
 

src/platform/kpi_rover_bringup/launch/sim.launch.py

@@ -1,168 +1,52 @@
 from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler)
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, SetLaunchConfiguration
 from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
-from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 
+from kpi_rover_worlds import world_info
+
+
+def start_pose(context):
+    cfg = context.launch_configurations
+    if not cfg['start']:
+        return []
+    world = world_info.build(cfg['world'], {})
+    x, y, z, yaw = world_info.start_pose(world, cfg['start'])
+    # 3 cm above the start point, so the robot drops onto uneven ground instead of into it
+    pose = zip(('x_pose', 'y_pose', 'z_pose', 'yaw_pose'), (x, y, z + 0.03, yaw))
+    return [SetLaunchConfiguration('world', world)] + [SetLaunchConfiguration(k, str(v)) for k, v in pose]
+
 
 def generate_launch_description():
     pkg_bringup = FindPackageShare('kpi_rover_bringup')
-    pkg_description = FindPackageShare('kpi_rover_description')
-    pkg_ros_gz_sim = FindPackageShare('ros_gz_sim')
 
-    world = LaunchConfiguration('world')
-    wheel_radius = LaunchConfiguration('wheel_radius')
-    x_pose = LaunchConfiguration('x_pose')
-    y_pose = LaunchConfiguration('y_pose')
-    z_pose = LaunchConfiguration('z_pose')
-    gui = LaunchConfiguration('gui')
     rviz = LaunchConfiguration('rviz')
 
     declared_arguments = [
         DeclareLaunchArgument(
-            'world',
-            default_value=PathJoinSubstitution([pkg_bringup, 'worlds', 'room.world']),
-            description='Path to the Gazebo world file'),
-        DeclareLaunchArgument(
-            'wheel_radius', default_value='0.034',
-            description='Wheel radius in meters'),
-        DeclareLaunchArgument('x_pose', default_value='0.0', description='Spawn X position'),
-        DeclareLaunchArgument('y_pose', default_value='0.0', description='Spawn Y position'),
-        DeclareLaunchArgument('z_pose', default_value='0.1', description='Spawn Z position'),
+            'world', default_value='room',
+            description='World of kpi_rover_worlds (python3 -m kpi_rover_worlds.world_info) or path to a world file'),
         DeclareLaunchArgument(
-            'gui', default_value='true',
-            description='Start the Gazebo GUI (otherwise headless server only)'),
+            'start', default_value='',
+            description='Start point of the world to spawn the robot at, instead of the pose'),
         DeclareLaunchArgument(
             'rviz', default_value='true',
             description='Start RViz2'),
-        DeclareLaunchArgument(
-            'slam', default_value='true',
-            description='Start slam_toolbox'),
-        DeclareLaunchArgument(
-            'nav', default_value='true',
-            description='Start Nav2 (handles the RViz 2D Goal Pose)'),
         DeclareLaunchArgument(
             'teleop', default_value='true',
             description='Start joystick teleop'),
     ]
 
-    # Lets Gazebo resolve model://kpi_rover_description/meshes/...
-    gz_resource_path = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH',
-        PathJoinSubstitution([pkg_description, '..']),
-    )
-
-    # '-s' runs the server only (headless)
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'])),
-        launch_arguments={
-            'gz_args': [
-                '-r -v3 ',
-                PythonExpression(["'' if '", gui, "' == 'true' else '-s '"]),
-                world,
-            ],
-            'on_exit_shutdown': 'true',
-        }.items(),
-    )
-
-    # robot_state_publisher with the sim_mode URDF (gz_ros2_control plugin)
-    description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([pkg_description, 'launch', 'description.launch.py'])),
-        launch_arguments={
-            'use_sim_time': 'true',
-            'sim_mode': 'true',
-            'wheel_radius': wheel_radius,
-        }.items(),
-    )
-
-    spawn_robot = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=[
-            '-name', 'kpi_rover',
-            '-topic', 'robot_description',
-            '-x', x_pose,
-            '-y', y_pose,
-            '-z', z_pose,
-        ],
-        output='screen',
-    )
-
-    gz_bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        parameters=[{
-            'config_file': PathJoinSubstitution([pkg_bringup, 'config', 'gz_bridge.yaml']),
-        }],
-        output='screen',
-    )
-
-    joint_state_broadcaster = Node(
-        package='controller_manager',
-        executable='spawner',
-        arguments=['joint_state_broadcaster', '--switch-timeout', '60'],
-        output='screen',
-    )
-
-    diff_drive_controller = Node(
-        package='controller_manager',
-        executable='spawner',
-        arguments=[
-            'diff_drive_controller',
-            '--switch-timeout', '60',
-            '--controller-ros-args', '-r /diff_drive_controller/odom:=/odom',
-        ],
-        output='screen',
-    )
-
-    # The controller manager only runs once the robot model is in the world
-    spawn_controllers = RegisterEventHandler(OnProcessExit(
-        target_action=spawn_robot,
-        on_exit=[joint_state_broadcaster, diff_drive_controller],
-    ))
-
-    # Turns unstamped /cmd_vel (Nav2, teleop_twist_keyboard) into the stamped input of the controller
-    twist_stamper = Node(
-        package='twist_stamper',
-        executable='twist_stamper',
-        parameters=[{'use_sim_time': True, 'frame_id': 'base_link'}],
-        remappings=[
-            ('cmd_vel_in', '/cmd_vel'),
-            ('cmd_vel_out', '/diff_drive_controller/cmd_vel'),
-        ],
-        output='screen',
-    )
-
-    localization = IncludeLaunchDescription(
+    world = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare('kpi_rover_localization'), 'launch', 'localization.launch.py'])),
-        launch_arguments={'use_sim_time': 'true'}.items(),
-    )
-
-    slam = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare('kpi_rover_slam'), 'launch', 'slam.launch.py'])),
-        launch_arguments={'use_sim_time': 'true'}.items(),
-        condition=IfCondition(LaunchConfiguration('slam')),
+            PathJoinSubstitution([pkg_bringup, 'launch', 'world.launch.py'])),
     )
 
-    navigation = IncludeLaunchDescription(
+    robot = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py'])),
-        launch_arguments={
-            'use_sim_time': 'true',
-            'params_file': PathJoinSubstitution([pkg_bringup, 'config', 'navigation.yaml']),
-        }.items(),
-        condition=IfCondition(LaunchConfiguration('nav')),
+            PathJoinSubstitution([pkg_bringup, 'launch', 'sim_robot.launch.py'])),
     )
 
     teleop = IncludeLaunchDescription(
@@ -180,16 +64,9 @@ def generate_launch_description():
     )
 
     return LaunchDescription(declared_arguments + [
-        gz_resource_path,
-        gazebo,
-        description,
-        spawn_robot,
-        gz_bridge,
-        spawn_controllers,
-        twist_stamper,
-        localization,
-        slam,
-        navigation,
+        OpaqueFunction(function=start_pose),
+        world,
+        robot,
         teleop,
         rviz_launch,
     ])