Made sim fast
This commit is contained in:
wittenator
parent
5d58add40a
commit
dc3831d32f
@ -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()
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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"/> -->
|
||||
|
||||
@ -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 -->
|
||||
|
||||
@ -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>
|
||||
1
src/dcaiti_control/env-hooks/dcaiti_control.dsv.in
Normal file
1
src/dcaiti_control/env-hooks/dcaiti_control.dsv.in
Normal file
@ -0,0 +1 @@
|
||||
prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds
|
||||
@ -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 )
|
||||
])
|
||||
|
||||
84
src/dcaiti_control/worlds/empty.sdf.template
Normal file
84
src/dcaiti_control/worlds/empty.sdf.template
Normal file
@ -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>
|
||||
@ -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>
|
||||
208
src/dcaiti_control/worlds/generate_track_world.py
Normal file
208
src/dcaiti_control/worlds/generate_track_world.py
Normal file
@ -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)
|
||||
|
||||
1881
src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
Normal file
1881
src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf
Normal file
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user