Made sim fast

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()

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
+    wheelbase: 3.0
-    front_wheel_track: 2.12321
+    front_wheel_track: 3.0
-    rear_wheel_track: 1.76868
+    rear_wheel_track: 3.0
-    front_wheels_radius: 0.45
+    front_wheels_radius: 0.2
-    rear_wheels_radius: 0.45
+    rear_wheels_radius: 0.2
 
 
 joint_limits:

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"/>  -->

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_radius" default="0.2"/>
-    <xacro:arg name="wheel_width" default="0.02"/>
+    <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_length" value="3.0" /> 
-    <xacro:property name="base_width" value="10.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_radius" value="0.2" />
-    <xacro:property name="wheel_thickness" value="0.08" />
+    <xacro:property name="wheel_thickness" value="0.2" />
     <xacro:property name="wheel_mass" value="1" />
 
     <!-- Steering link dimensions -->

src/dcaiti_control/description/yellow_cone.xacro

@@ -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>

src/dcaiti_control/env-hooks/dcaiti_control.dsv.in

@@ -0,0 +1 @@
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds

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 ) 
+    ]) 

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>

src/dcaiti_control/worlds/empty.world

@@ -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>

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)
+