t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added first track

Browse Source
This commit is contained in:
wittenator
2023-10-29 19:41:14 +01:00
parent f581d25590
commit 5d58add40a
8 changed files with 546 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dcaiti_control/CMakeLists.txt
View File

@ -33,7 +33,7 @@ if(BUILD_TESTING)
endif()
install(
DIRECTORY config description launch worlds
DIRECTORY config description launch worlds tracks
DESTINATION share/${PROJECT_NAME}
)

326
src/dcaiti_control/description/assets/blue_cone.dae Normal file
View File

File diff suppressed because one or more lines are too long

4
src/dcaiti_control/description/assets/yellow_cone.dae
View File

@ -5,8 +5,8 @@
<author>Blender User</author>
<authoring_tool>Blender 3.3.11 commit date:2023-09-21, commit time:06:07, hash:6a3240da1dc8</authoring_tool>
</contributor>
<created>2023-10-29T16:28:48</created>
<modified>2023-10-29T16:28:48</modified>
<created>2023-10-29T19:26:22</created>
<modified>2023-10-29T19:26:22</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>

24
src/dcaiti_control/description/blue_cone.xacro Normal file
View File

@ -0,0 +1,24 @@
<?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/blue_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>

10
src/dcaiti_control/description/yellow_cone.xacro
View File

@ -3,12 +3,12 @@
<xacro:include filename="inertial_macros.xacro"/>
<gazebo>
<!-- make static-->
<static>true</static>
</gazebo>
<link name="cone">
<collision>
<geometry>
<cylinder radius="0.112" length="0.3"/>
</geometry>
</collision>
<visual>
<geometry>
<!-- <cylinder radius="0.112" length="0.3"/> -->

203
src/dcaiti_control/launch/launch_sim.py
View File

@ -1,4 +1,5 @@
import os
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
@ -16,6 +17,139 @@ 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():
@ -25,15 +159,17 @@ def generate_launch_description():
package_name='dcaiti_control' #<--- CHANGE ME
use_ros2_control = LaunchConfiguration('use_ros2_control')
track = LaunchConfiguration('track')
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'true', 'use_ros2_control': use_ros2_control}.items()
)
gazebo_params_path = os.path.join(get_package_share_directory(package_name),'config','gazebo_config.yml')
world_path = os.path.join(get_package_share_directory(package_name),'worlds')
description_path = os.path.join(get_package_share_directory(package_name),'description')
base_path = Path(get_package_share_directory(package_name))
world_path = base_path / 'worlds'
description_path = base_path /'description'
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
@ -48,20 +184,54 @@ def generate_launch_description():
'-topic', 'robot_description',
'-name', 'spawn_robot',
'-z', '0.5',
'-x', '-7',
],
output='screen'
)
cone_xacro = f'{description_path}/yellow_cone.xacro'
cone_config = Command(['xacro ', cone_xacro])
spawn_cone = Node(package='ros_gz_sim', executable='create',
arguments=[
'-string', cone_config,
'-name', 'spawned_cone',
'-z', '10',
],
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(
@ -111,11 +281,14 @@ def generate_launch_description():
'use_ros2_control',
default_value='true',
description='Use ros2_control if true'),
DeclareLaunchArgument(
'track',
default_value='AU2_skidpad.lyt',
description='Which track to load'),
bridge,
rsp,
gazebo,
spawn_entity,
spawn_cone,
delayed_diff_drive_spawner,
delayed_joint_broad_spawner,
])
]+ cone_spawner )

176
src/dcaiti_control/track/readlyt.py
View File

@ -1,176 +0,0 @@
#!/usr/bin/env python3
# -*- coding:utf-8 -*-
"""
Description: Based on https://www.lfs.net/programmer/lyt
"""
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 main():
# parse arguments
import argparse
import json
# 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_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(),
}
# print the cones as json
print(json.dumps(cones_dict, indent=1))
if __name__ == "__main__":
main()

0
src/dcaiti_control/track/AU2_skidpad.lyt → src/dcaiti_control/tracks/AU2_skidpad.lyt
View File