From dc3831d32ff0ab1a69e1a17d55abcc3e880e6cb1 Mon Sep 17 00:00:00 2001
From: wittenator <9154515+wittenator@users.noreply.github.com>
Date: Mon, 30 Oct 2023 15:14:23 +0100
Subject: [PATCH] Made sim fast
---
src/dcaiti_control/CMakeLists.txt | 3 +-
src/dcaiti_control/config/dcaiti_config.yml | 10 +-
.../description/blue_cone.xacro | 10 +-
.../description/robot.urdf.xacro | 12 +-
.../description/yellow_cone.xacro | 24 -
.../env-hooks/dcaiti_control.dsv.in | 1 +
src/dcaiti_control/launch/launch_sim.py | 184 +-
.../assets/blue_cone.dae | 0
.../assets/yellow_cone.dae | 0
src/dcaiti_control/worlds/empty.sdf.template | 84 +
src/dcaiti_control/worlds/empty.world | 13 -
.../worlds/generate_track_world.py | 208 ++
.../worlds/generated_worlds/AU2_skidpad.sdf | 1881 +++++++++++++++++
.../{ => worlds}/tracks/AU2_skidpad.lyt | Bin
14 files changed, 2194 insertions(+), 236 deletions(-)
delete mode 100644 src/dcaiti_control/description/yellow_cone.xacro
create mode 100644 src/dcaiti_control/env-hooks/dcaiti_control.dsv.in
rename src/dcaiti_control/{description => worlds}/assets/blue_cone.dae (100%)
rename src/dcaiti_control/{description => worlds}/assets/yellow_cone.dae (100%)
create mode 100644 src/dcaiti_control/worlds/empty.sdf.template
delete mode 100644 src/dcaiti_control/worlds/empty.world
create mode 100644 src/dcaiti_control/worlds/generate_track_world.py
create mode 100644 src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
rename src/dcaiti_control/{ => worlds}/tracks/AU2_skidpad.lyt (100%)
diff --git a/src/dcaiti_control/CMakeLists.txt b/src/dcaiti_control/CMakeLists.txt
index 96da4cb..0875add 100644
--- a/src/dcaiti_control/CMakeLists.txt
+++ b/src/dcaiti_control/CMakeLists.txt
@@ -33,8 +33,9 @@ if(BUILD_TESTING)
endif()
install(
- DIRECTORY config description launch worlds tracks
+ DIRECTORY config description launch worlds
DESTINATION share/${PROJECT_NAME}
)
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
ament_package()
diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/dcaiti_control/config/dcaiti_config.yml
index 46ca605..f04465e 100644
--- a/src/dcaiti_control/config/dcaiti_config.yml
+++ b/src/dcaiti_control/config/dcaiti_config.yml
@@ -25,11 +25,11 @@ ackermann_steering_controller:
rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
front_wheels_names: [front_right_steer_joint, front_left_steer_joint]
- wheelbase: 3.24644
- front_wheel_track: 2.12321
- rear_wheel_track: 1.76868
- front_wheels_radius: 0.45
- rear_wheels_radius: 0.45
+ wheelbase: 3.0
+ front_wheel_track: 3.0
+ rear_wheel_track: 3.0
+ front_wheels_radius: 0.2
+ rear_wheels_radius: 0.2
joint_limits:
diff --git a/src/dcaiti_control/description/blue_cone.xacro b/src/dcaiti_control/description/blue_cone.xacro
index 8fd54e3..cf90b90 100644
--- a/src/dcaiti_control/description/blue_cone.xacro
+++ b/src/dcaiti_control/description/blue_cone.xacro
@@ -3,12 +3,12 @@
<xacro:include filename="inertial_macros.xacro"/>
- <gazebo>
- <!-- make static-->
- <static>true</static>
- </gazebo>
-
<link name="cone">
+ <collision>
+ <geometry>
+ <box size="0.112 0.112 0.3"/>
+ </geometry>
+ </collision>
<visual>
<geometry>
<!-- <cylinder radius="0.112" length="0.3"/> -->
diff --git a/src/dcaiti_control/description/robot.urdf.xacro b/src/dcaiti_control/description/robot.urdf.xacro
index da9598c..994d34d 100644
--- a/src/dcaiti_control/description/robot.urdf.xacro
+++ b/src/dcaiti_control/description/robot.urdf.xacro
@@ -13,21 +13,21 @@
<xacro:arg name="lidar_height" default="0.201"/>
- <xacro:arg name="wheel_radius" default="0.05"/>
- <xacro:arg name="wheel_width" default="0.02"/>
+ <xacro:arg name="wheel_radius" default="0.2"/>
+ <xacro:arg name="wheel_width" default="0.2"/>
<!-- Math constants -->
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<!-- Robot base dimensions -->
- <xacro:property name="base_length" value="10.0" />
- <xacro:property name="base_width" value="10.0" />
+ <xacro:property name="base_length" value="3.0" />
+ <xacro:property name="base_width" value="2.0" />
<xacro:property name="base_height" value="0.1" />
<xacro:property name="base_mass" value="5.0" />
<!-- Wheel link dimensions -->
- <xacro:property name="wheel_radius" value="0.1" />
- <xacro:property name="wheel_thickness" value="0.08" />
+ <xacro:property name="wheel_radius" value="0.2" />
+ <xacro:property name="wheel_thickness" value="0.2" />
<xacro:property name="wheel_mass" value="1" />
<!-- Steering link dimensions -->
diff --git a/src/dcaiti_control/description/yellow_cone.xacro b/src/dcaiti_control/description/yellow_cone.xacro
deleted file mode 100644
index e1ecf39..0000000
--- a/src/dcaiti_control/description/yellow_cone.xacro
+++ /dev/null
@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="cone">
-
- <xacro:include filename="inertial_macros.xacro"/>
-
- <gazebo>
- <!-- make static-->
- <static>true</static>
- </gazebo>
-
- <link name="cone">
- <visual>
- <geometry>
- <!-- <cylinder radius="0.112" length="0.3"/> -->
- <mesh filename="file://$(find dcaiti_control)/description/assets/yellow_cone.dae"/>
- </geometry>
- </visual>
- <xacro:solid_cylinder_inertial
- rpy="0 0 0" xyz="0 0 0"
- mass="0.5"
- radius="0.112" length="0.3" />
- </link>
-
-</robot>
\ No newline at end of file
diff --git a/src/dcaiti_control/env-hooks/dcaiti_control.dsv.in b/src/dcaiti_control/env-hooks/dcaiti_control.dsv.in
new file mode 100644
index 0000000..b604faf
--- /dev/null
+++ b/src/dcaiti_control/env-hooks/dcaiti_control.dsv.in
@@ -0,0 +1 @@
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds
\ No newline at end of file
diff --git a/src/dcaiti_control/launch/launch_sim.py b/src/dcaiti_control/launch/launch_sim.py
index e7ae612..ddc09f2 100644
--- a/src/dcaiti_control/launch/launch_sim.py
+++ b/src/dcaiti_control/launch/launch_sim.py
@@ -17,140 +17,6 @@ from launch_ros.actions import Node
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessExit
-from pathlib import Path
-from typing import List, Tuple, Union, cast, Dict
-from struct import Struct
-from enum import IntEnum
-
-import numpy as np
-
-
-
-class ConeTypes(IntEnum):
- """
- Enum for all possible cone types
- """
-
- UNKNOWN = 0
- YELLOW = 1
- RIGHT = 1
- BLUE = 2
- LEFT = 2
- ORANGE_SMALL = 3
- START_FINISH_AREA = 3
- ORANGE_BIG = 4
- START_FINISH_LINE = 4
-
-
-HEADER_STRUCT = Struct("6sBBhBB")
-BLOCK_STRUCT = Struct("2h4B")
-
-LytObjectIndexToConeType: Dict[int, ConeTypes] = {
- 25: ConeTypes.UNKNOWN,
- 29: ConeTypes.YELLOW,
- 30: ConeTypes.YELLOW,
- 23: ConeTypes.BLUE,
- 24: ConeTypes.BLUE,
- 27: ConeTypes.ORANGE_BIG,
- 20: ConeTypes.ORANGE_SMALL,
-}
-
-def split_header_blocks(data: bytes) -> Tuple[bytes, bytes]:
- """
- Split the content of the lyt file into header and block. This split is easy because
- the header has a fixed size
-
- Args:
- data (bytes): The content of the lyt file
-
- Returns:
- Tuple[bytes, bytes]: The header and the block
- """
- return data[: HEADER_STRUCT.size], data[HEADER_STRUCT.size :]
-
-
-def verify_lyt_header(header_data: bytes) -> None:
- """
- Parse the header and perform some sanity checks suggested by the LFS documentation
-
- Args:
- header_data (bytes): The header bytes of the `.lyt` file
- """
-
- header = cast(
- Tuple[bytes, int, int, int, int, int], HEADER_STRUCT.unpack(header_data)
- )
-
- file_type, version, revision, _, _, _ = header
- assert file_type == b"LFSLYT"
- assert version <= 0, version
- # revision allowed up to 252
- # https://www.lfs.net/forum/thread/96153-LYT-revision-252-in-W-patch
- assert revision <= 252, revision
-
-
-def extract_cone_lists(blocks_data: bytes) -> List[List[Tuple[float, float]]]:
- """
- Extract the cone object positions from the object blocks bytes of a lyt file
-
- Args:
- blocks_data (bytes): The data in the lyt file that is not the header
-
- Returns:
- List[List[Tuple[int, int]]]: The cone positions split by cone type
- """
- decoded_blocks = BLOCK_STRUCT.iter_unpack(blocks_data)
- all_cones_per_type: List[List[Tuple[float, float]]] = [[] for _ in ConeTypes]
-
- # cone_info:
- for cone_info in decoded_blocks:
- obj_x, obj_y, _, _, lyt_obj_idx, _ = cast(
- Tuple[int, int, int, int, int, int], cone_info
- )
-
- try:
- cone_type = LytObjectIndexToConeType[lyt_obj_idx]
- except KeyError:
- # not a cone
- continue
-
- # the stored x,y pos is multiplied by
- # 16 in the file so we need to convert it back
- # (and cast to a float by using real div)
- obj_x_meters = obj_x / 16
- obj_y_meters = obj_y / 16
- all_cones_per_type[cone_type].append((obj_x_meters, obj_y_meters))
- return all_cones_per_type
-
-
-def load_lyt_file(filename: Union[Path, str]) -> List[np.ndarray]:
- """
- Load a `.lyt` file and return the positions of the cone objects inside it split
- according to `ConeTypes`
-
- Args:
- filename (Path): The path to the `.lyt` file
-
- Returns:
- List[np.ndarray]: A list of 2d np.ndarrays representing the cone positions of
- for all cone types
- """
- if isinstance(filename, str):
- filename = Path(filename)
- assert filename.is_file(), filename
- assert filename.suffix == ".lyt", filename
- data = filename.read_bytes()
- header_data, blocks_data = split_header_blocks(data)
- verify_lyt_header(header_data)
-
- all_cones_per_type = extract_cone_lists(blocks_data)
-
- all_cones_per_type_arrays = [
- np.array(cone_list) for cone_list in all_cones_per_type
- ]
-
- return all_cones_per_type_arrays
-
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
@@ -175,7 +41,7 @@ def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
- launch_arguments=[('gz_args', [f" -r -v 1 {world_path}/empty.sdf"])],
+ launch_arguments=[('gz_args', [f" -r -v 1 {world_path}/generated_worlds/AU2_skidpad.sdf"])],
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
@@ -189,50 +55,6 @@ def generate_launch_description():
output='screen'
)
- cone_positions = [x.reshape(-1,2) for x in load_lyt_file(base_path / 'tracks' / 'AU2_skidpad.lyt')]
- center = np.mean(cone_positions[ConeTypes.ORANGE_BIG], axis=0)
-
- cone_positions_centered = [x - center for x in cone_positions]
-
-
- cones_dict = {
- "unknown": cone_positions_centered[ConeTypes.UNKNOWN].tolist(),
- "yellow": cone_positions_centered[ConeTypes.YELLOW].tolist(),
- "blue": cone_positions_centered[ConeTypes.BLUE].tolist(),
- "orange_small": cone_positions_centered[ConeTypes.ORANGE_SMALL].tolist(),
- "orange_big": cone_positions_centered[ConeTypes.ORANGE_BIG].tolist(),
- }
-
- yellow_cone_xacro = f'{description_path}/yellow_cone.xacro'
- yellow_cone_config = Command(['xacro ', yellow_cone_xacro])
- blue_cone_xacro = f'{description_path}/blue_cone.xacro'
- blue_cone_config = Command(['xacro ', blue_cone_xacro])
- cone_spawner = []
- for i, (x,y) in enumerate(cones_dict['yellow']):
- spawn_cone = Node(package='ros_gz_sim', executable='create',
- arguments=[
- '-string', yellow_cone_config,
- '-name', f'yellow_cone_{i}',
- '-z', '0.3',
- '-y', str(y),
- '-x', str(x),
- ],
- output='screen'
- )
- cone_spawner.append(spawn_cone)
- for i, (x,y) in enumerate(cones_dict['blue']):
- spawn_cone = Node(package='ros_gz_sim', executable='create',
- arguments=[
- '-string', blue_cone_config,
- '-name', f'blue_cone_{i}',
- '-z', '0.3',
- '-y', str(y),
- '-x', str(x),
- ],
- output='screen'
- )
- cone_spawner.append(spawn_cone)
-
# Bridge
bridge = Node(
package='ros_gz_bridge',
@@ -240,8 +62,6 @@ def generate_launch_description():
arguments=[
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
'/boxes@vision_msgs/msg/Detection2DArray@ignition.msgs.AnnotatedAxisAligned2DBox_V',
- '/boxes_image@sensor_msgs/msg/Image@ignition.msgs.Image',
- '/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo',
'/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
'/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked'
],
@@ -291,4 +111,4 @@ def generate_launch_description():
spawn_entity,
delayed_diff_drive_spawner,
delayed_joint_broad_spawner,
- ]+ cone_spawner )
+ ])
diff --git a/src/dcaiti_control/description/assets/blue_cone.dae b/src/dcaiti_control/worlds/assets/blue_cone.dae
similarity index 100%
rename from src/dcaiti_control/description/assets/blue_cone.dae
rename to src/dcaiti_control/worlds/assets/blue_cone.dae
diff --git a/src/dcaiti_control/description/assets/yellow_cone.dae b/src/dcaiti_control/worlds/assets/yellow_cone.dae
similarity index 100%
rename from src/dcaiti_control/description/assets/yellow_cone.dae
rename to src/dcaiti_control/worlds/assets/yellow_cone.dae
diff --git a/src/dcaiti_control/worlds/empty.sdf.template b/src/dcaiti_control/worlds/empty.sdf.template
new file mode 100644
index 0000000..3329b81
--- /dev/null
+++ b/src/dcaiti_control/worlds/empty.sdf.template
@@ -0,0 +1,84 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+ <world name="empty">
+ <physics name="phys" type="ode">
+ <max_step_size>0.005</max_step_size>
+ <real_time_factor>1.0</real_time_factor>
+ <ode>
+ <solver>
+ <type>quick</type>
+ <island_threads>true</island_threads>
+ <thread_position_correction>true</thread_position_correction>
+ <friction_model>cone_model</friction_model>
+ </solver>
+ </ode>
+ </physics>
+ <plugin
+ filename="ignition-gazebo-physics-system"
+ name="gz::sim::systems::Physics">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-user-commands-system"
+ name="gz::sim::systems::UserCommands">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-scene-broadcaster-system"
+ name="gz::sim::systems::SceneBroadcaster">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-contact-system"
+ name="gz::sim::systems::Contact">
+ </plugin>
+ <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+ <render_engine>ogre2</render_engine>
+ </plugin>
+ <scene>
+ <shadows>false</shadows>
+ <grid>false</grid>
+ <origin_visual>false</origin_visual>
+ </scene>
+
+ <light type="directional" name="sun">
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <ambient>0.8 0.8 0.8 1</ambient>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ {{cones}}
+
+ </world>
+</sdf>
\ No newline at end of file
diff --git a/src/dcaiti_control/worlds/empty.world b/src/dcaiti_control/worlds/empty.world
deleted file mode 100644
index 5321672..0000000
--- a/src/dcaiti_control/worlds/empty.world
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
- <world name="default">
- <!-- A global light source -->
- <include>
- <uri>model://sun</uri>
- </include>
- <!-- A ground plane -->
- <include>
- <uri>model://ground_plane</uri>
- </include>
- </world>
-</sdf>
diff --git a/src/dcaiti_control/worlds/generate_track_world.py b/src/dcaiti_control/worlds/generate_track_world.py
new file mode 100644
index 0000000..8c3f916
--- /dev/null
+++ b/src/dcaiti_control/worlds/generate_track_world.py
@@ -0,0 +1,208 @@
+from pathlib import Path
+from typing import List, Tuple, Union, cast, Dict
+from struct import Struct
+from enum import IntEnum
+
+import numpy as np
+
+
+
+class ConeTypes(IntEnum):
+ """
+ Enum for all possible cone types
+ """
+
+ UNKNOWN = 0
+ YELLOW = 1
+ RIGHT = 1
+ BLUE = 2
+ LEFT = 2
+ ORANGE_SMALL = 3
+ START_FINISH_AREA = 3
+ ORANGE_BIG = 4
+ START_FINISH_LINE = 4
+
+
+HEADER_STRUCT = Struct("6sBBhBB")
+BLOCK_STRUCT = Struct("2h4B")
+
+LytObjectIndexToConeType: Dict[int, ConeTypes] = {
+ 25: ConeTypes.UNKNOWN,
+ 29: ConeTypes.YELLOW,
+ 30: ConeTypes.YELLOW,
+ 23: ConeTypes.BLUE,
+ 24: ConeTypes.BLUE,
+ 27: ConeTypes.ORANGE_BIG,
+ 20: ConeTypes.ORANGE_SMALL,
+}
+
+def split_header_blocks(data: bytes) -> Tuple[bytes, bytes]:
+ """
+ Split the content of the lyt file into header and block. This split is easy because
+ the header has a fixed size
+
+ Args:
+ data (bytes): The content of the lyt file
+
+ Returns:
+ Tuple[bytes, bytes]: The header and the block
+ """
+ return data[: HEADER_STRUCT.size], data[HEADER_STRUCT.size :]
+
+
+def verify_lyt_header(header_data: bytes) -> None:
+ """
+ Parse the header and perform some sanity checks suggested by the LFS documentation
+
+ Args:
+ header_data (bytes): The header bytes of the `.lyt` file
+ """
+
+ header = cast(
+ Tuple[bytes, int, int, int, int, int], HEADER_STRUCT.unpack(header_data)
+ )
+
+ file_type, version, revision, _, _, _ = header
+ assert file_type == b"LFSLYT"
+ assert version <= 0, version
+ # revision allowed up to 252
+ # https://www.lfs.net/forum/thread/96153-LYT-revision-252-in-W-patch
+ assert revision <= 252, revision
+
+
+def extract_cone_lists(blocks_data: bytes) -> List[List[Tuple[float, float]]]:
+ """
+ Extract the cone object positions from the object blocks bytes of a lyt file
+
+ Args:
+ blocks_data (bytes): The data in the lyt file that is not the header
+
+ Returns:
+ List[List[Tuple[int, int]]]: The cone positions split by cone type
+ """
+ decoded_blocks = BLOCK_STRUCT.iter_unpack(blocks_data)
+ all_cones_per_type: List[List[Tuple[float, float]]] = [[] for _ in ConeTypes]
+
+ # cone_info:
+ for cone_info in decoded_blocks:
+ obj_x, obj_y, _, _, lyt_obj_idx, _ = cast(
+ Tuple[int, int, int, int, int, int], cone_info
+ )
+
+ try:
+ cone_type = LytObjectIndexToConeType[lyt_obj_idx]
+ except KeyError:
+ # not a cone
+ continue
+
+ # the stored x,y pos is multiplied by
+ # 16 in the file so we need to convert it back
+ # (and cast to a float by using real div)
+ obj_x_meters = obj_x / 16
+ obj_y_meters = obj_y / 16
+ all_cones_per_type[cone_type].append((obj_x_meters, obj_y_meters))
+ return all_cones_per_type
+
+
+def load_lyt_file(filename: Union[Path, str]) -> List[np.ndarray]:
+ """
+ Load a `.lyt` file and return the positions of the cone objects inside it split
+ according to `ConeTypes`
+
+ Args:
+ filename (Path): The path to the `.lyt` file
+
+ Returns:
+ List[np.ndarray]: A list of 2d np.ndarrays representing the cone positions of
+ for all cone types
+ """
+ if isinstance(filename, str):
+ filename = Path(filename)
+ assert filename.is_file(), filename
+ assert filename.suffix == ".lyt", filename
+ data = filename.read_bytes()
+ header_data, blocks_data = split_header_blocks(data)
+ verify_lyt_header(header_data)
+
+ all_cones_per_type = extract_cone_lists(blocks_data)
+
+ all_cones_per_type_arrays = [
+ np.array(cone_list) for cone_list in all_cones_per_type
+ ]
+
+ return all_cones_per_type_arrays
+
+if __name__ == "__main__":
+ import argparse
+ import json
+ import jinja2
+
+ # one argument for the lyt file
+ parser = argparse.ArgumentParser(description="Parse a lyt file")
+ parser.add_argument("lyt_file", type=str, help="The lyt file to parse")
+
+ args = parser.parse_args()
+
+ # load the lyt file
+ lyt_file = Path(args.lyt_file)
+ cone_positions = [x.reshape(-1,2) for x in load_lyt_file(lyt_file)]
+
+
+ center = np.mean(cone_positions[ConeTypes.ORANGE_BIG], axis=0)
+
+ cone_positions_centered = [x - center for x in cone_positions]
+
+
+ cones_dict = {
+ "unknown": cone_positions_centered[ConeTypes.UNKNOWN].tolist(),
+ "yellow_cone": cone_positions_centered[ConeTypes.YELLOW].tolist(),
+ "blue_cone": cone_positions_centered[ConeTypes.BLUE].tolist(),
+ "orange_small_cone": cone_positions_centered[ConeTypes.ORANGE_SMALL].tolist(),
+ "orange_big_cone": cone_positions_centered[ConeTypes.ORANGE_BIG].tolist(),
+ }
+
+ cone_list_sdf = []
+ z = 1
+
+ for cone_type in ['blue_cone', 'yellow_cone']:
+ for i, (x,y) in enumerate(cones_dict[cone_type]):
+ cone_sdf = f'''
+ <model name="{cone_type}_{i}">
+ <link name="link">
+ <pose>{x} {y} {z} 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/{cone_type}.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+ '''
+ cone_list_sdf.append(cone_sdf)
+ # read template and fill in the cone positions
+ template = jinja2.Template(Path("empty.sdf.template").read_text())
+ filled_template = template.render(cones="\n".join(cone_list_sdf))
+
+ # write the filled template string to a sdf file
+ with open(lyt_file.parent.parent / "generated_worlds" / lyt_file.with_suffix('.sdf').name, "w+") as f:
+ f.write(filled_template)
+
diff --git a/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf b/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
new file mode 100644
index 0000000..a9df3cc
--- /dev/null
+++ b/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
@@ -0,0 +1,1881 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+ <world name="empty">
+ <physics name="phys" type="ode">
+ <max_step_size>0.005</max_step_size>
+ <real_time_factor>1.0</real_time_factor>
+ <ode>
+ <solver>
+ <type>quick</type>
+ <island_threads>true</island_threads>
+ <thread_position_correction>true</thread_position_correction>
+ <friction_model>cone_model</friction_model>
+ </solver>
+ </ode>
+ </physics>
+ <plugin
+ filename="ignition-gazebo-physics-system"
+ name="gz::sim::systems::Physics">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-user-commands-system"
+ name="gz::sim::systems::UserCommands">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-scene-broadcaster-system"
+ name="gz::sim::systems::SceneBroadcaster">
+ </plugin>
+ <plugin
+ filename="ignition-gazebo-contact-system"
+ name="gz::sim::systems::Contact">
+ </plugin>
+ <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+ <render_engine>ogre2</render_engine>
+ </plugin>
+ <scene>
+ <shadows>false</shadows>
+ <grid>false</grid>
+ <origin_visual>false</origin_visual>
+ </scene>
+
+ <light type="directional" name="sun">
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <ambient>0.8 0.8 0.8 1</ambient>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+
+ <model name="blue_cone_0">
+ <link name="link">
+ <pose>-2.078125 2.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_1">
+ <link name="link">
+ <pose>-3.703125 5.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_2">
+ <link name="link">
+ <pose>-6.203125 7.09375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_3">
+ <link name="link">
+ <pose>-9.078125 7.65625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_4">
+ <link name="link">
+ <pose>-12.015625 7.09375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_5">
+ <link name="link">
+ <pose>-14.515625 5.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_6">
+ <link name="link">
+ <pose>-16.140625 2.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_7">
+ <link name="link">
+ <pose>-16.703125 0.09375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_8">
+ <link name="link">
+ <pose>-16.140625 -2.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_9">
+ <link name="link">
+ <pose>-14.515625 -5.34375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_10">
+ <link name="link">
+ <pose>-12.015625 -6.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_11">
+ <link name="link">
+ <pose>-9.140625 -7.53125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_12">
+ <link name="link">
+ <pose>-6.203125 -6.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_13">
+ <link name="link">
+ <pose>-3.703125 -5.34375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_14">
+ <link name="link">
+ <pose>-2.078125 -2.90625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_15">
+ <link name="link">
+ <pose>3.359375 -8.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_16">
+ <link name="link">
+ <pose>4.984375 -9.71875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_17">
+ <link name="link">
+ <pose>9.109375 -10.59375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_18">
+ <link name="link">
+ <pose>13.109375 -9.78125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_19">
+ <link name="link">
+ <pose>16.671875 -7.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_20">
+ <link name="link">
+ <pose>18.984375 -3.90625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_21">
+ <link name="link">
+ <pose>19.734375 0.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_22">
+ <link name="link">
+ <pose>18.984375 4.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_23">
+ <link name="link">
+ <pose>16.671875 7.59375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_24">
+ <link name="link">
+ <pose>13.109375 9.90625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_25">
+ <link name="link">
+ <pose>9.109375 10.65625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_26">
+ <link name="link">
+ <pose>4.984375 9.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_27">
+ <link name="link">
+ <pose>3.359375 8.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="blue_cone_28">
+ <link name="link">
+ <pose>-1.515625 -0.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/blue_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_0">
+ <link name="link">
+ <pose>-3.328125 -8.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_1">
+ <link name="link">
+ <pose>2.109375 -2.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_2">
+ <link name="link">
+ <pose>3.734375 -5.34375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_3">
+ <link name="link">
+ <pose>6.234375 -6.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_4">
+ <link name="link">
+ <pose>9.109375 -7.53125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_5">
+ <link name="link">
+ <pose>12.046875 -6.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_6">
+ <link name="link">
+ <pose>14.546875 -5.34375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_7">
+ <link name="link">
+ <pose>16.171875 -2.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_8">
+ <link name="link">
+ <pose>16.734375 0.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_9">
+ <link name="link">
+ <pose>16.171875 2.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_10">
+ <link name="link">
+ <pose>14.546875 5.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_11">
+ <link name="link">
+ <pose>12.046875 7.09375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_12">
+ <link name="link">
+ <pose>9.171875 7.65625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_13">
+ <link name="link">
+ <pose>6.234375 7.09375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_14">
+ <link name="link">
+ <pose>3.734375 5.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_15">
+ <link name="link">
+ <pose>2.109375 3.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_16">
+ <link name="link">
+ <pose>-3.328125 8.96875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_17">
+ <link name="link">
+ <pose>-4.953125 9.84375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_18">
+ <link name="link">
+ <pose>-9.140625 10.65625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_19">
+ <link name="link">
+ <pose>-13.078125 9.90625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_20">
+ <link name="link">
+ <pose>-16.640625 7.59375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_21">
+ <link name="link">
+ <pose>-18.953125 4.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_22">
+ <link name="link">
+ <pose>-19.765625 0.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_23">
+ <link name="link">
+ <pose>-18.953125 -3.90625 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_24">
+ <link name="link">
+ <pose>-16.640625 -7.46875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_25">
+ <link name="link">
+ <pose>-13.078125 -9.78125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_26">
+ <link name="link">
+ <pose>-9.140625 -10.59375 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_27">
+ <link name="link">
+ <pose>-4.953125 -9.71875 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ <model name="yellow_cone_28">
+ <link name="link">
+ <pose>1.484375 -0.03125 1 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.112 0.112 0.3</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <mesh><uri>model://assets/yellow_cone.dae</uri></mesh>
+ </geometry>
+ </visual>
+ <sensor name='sensor_contact' type='contact'>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ </link>
+ <plugin filename="libignition-gazebo-touchplugin-system.so"
+ name="ignition::gazebo::systems::TouchPlugin">
+ <target>vehicle_blue</target>
+ <namespace>wall</namespace>
+ <time>0.001</time>
+ <enabled>true</enabled>
+ </plugin>
+ </model>
+
+
+ </world>
+</sdf>
\ No newline at end of file
diff --git a/src/dcaiti_control/tracks/AU2_skidpad.lyt b/src/dcaiti_control/worlds/tracks/AU2_skidpad.lyt
similarity index 100%
rename from src/dcaiti_control/tracks/AU2_skidpad.lyt
rename to src/dcaiti_control/worlds/tracks/AU2_skidpad.lyt