394 lines
13 KiB
Python
394 lines
13 KiB
Python
from dataclasses import dataclass
|
|
import os
|
|
from returns.result import Result, Success, Failure
|
|
from typing import Optional, Any, List, TypeVar, Callable, Type, cast
|
|
from enum import Enum
|
|
|
|
from repository.file_system import FileSystemRepository
|
|
|
|
T = TypeVar("T")
|
|
EnumT = TypeVar("EnumT", bound=Enum)
|
|
|
|
|
|
def from_float(x: Any) -> float:
|
|
assert isinstance(x, (float, int)) and not isinstance(x, bool)
|
|
return float(x)
|
|
|
|
|
|
def from_none(x: Any) -> Any:
|
|
return x
|
|
|
|
|
|
def from_union(fs, x):
|
|
for f in fs:
|
|
try:
|
|
return f(x)
|
|
except:
|
|
pass
|
|
assert False
|
|
|
|
|
|
def to_float(x: Any) -> float:
|
|
assert isinstance(x, float)
|
|
return x
|
|
|
|
|
|
def from_str(x: Any) -> str:
|
|
assert isinstance(x, str)
|
|
return x
|
|
|
|
|
|
def from_int(x: Any) -> int:
|
|
assert isinstance(x, int) and not isinstance(x, bool)
|
|
return x
|
|
|
|
|
|
def from_list(f: Callable[[Any], T], x: Any) -> List[T]:
|
|
assert isinstance(x, list)
|
|
return [f(y) for y in x]
|
|
|
|
|
|
def to_class(c: Type[T], x: Any) -> dict:
|
|
assert isinstance(x, c)
|
|
return cast(Any, x).to_dict()
|
|
|
|
|
|
def to_enum(c: Type[EnumT], x: Any) -> EnumT:
|
|
assert isinstance(x, c)
|
|
return x.value
|
|
|
|
|
|
@dataclass
|
|
class Model:
|
|
name: Optional[str] = None
|
|
id: Optional[str] = None
|
|
path: Optional[str] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "Model":
|
|
assert isinstance(obj, dict)
|
|
name = from_union([from_str, from_none], obj.get("name"))
|
|
id = from_union([from_str, from_none], obj.get("id"))
|
|
path = from_union([from_str, from_none], obj.get("path"))
|
|
return Model(name, id, path)
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.name is not None:
|
|
result["name"] = from_union([from_str, from_none], self.name)
|
|
if self.id is not None:
|
|
result["id"] = from_union([from_str, from_none], self.id)
|
|
if self.path is not None:
|
|
result["path"] = from_union([from_str, from_none], self.path)
|
|
return result
|
|
|
|
|
|
@dataclass
|
|
class Pose:
|
|
x: Optional[float] = None
|
|
y: Optional[float] = None
|
|
z: Optional[float] = None
|
|
roll: Optional[float] = None
|
|
pitch: Optional[float] = None
|
|
yaw: Optional[float] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "Pose":
|
|
assert isinstance(obj, dict)
|
|
x = from_union([from_float, from_none], obj.get("x"))
|
|
y = from_union([from_float, from_none], obj.get("y"))
|
|
z = from_union([from_float, from_none], obj.get("z"))
|
|
roll = from_union([from_float, from_none], obj.get("roll"))
|
|
pitch = from_union([from_float, from_none], obj.get("pitch"))
|
|
yaw = from_union([from_float, from_none], obj.get("yaw"))
|
|
return Pose(x, y, z, roll, pitch, yaw)
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.x is not None:
|
|
result["x"] = from_union([to_float, from_none], self.x)
|
|
if self.y is not None:
|
|
result["y"] = from_union([to_float, from_none], self.y)
|
|
if self.z is not None:
|
|
result["z"] = from_union([to_float, from_none], self.z)
|
|
if self.roll is not None:
|
|
result["roll"] = from_union([to_float, from_none], self.roll)
|
|
if self.pitch is not None:
|
|
result["pitch"] = from_union([to_float, from_none], self.pitch)
|
|
if self.yaw is not None:
|
|
result["yaw"] = from_union([to_float, from_none], self.yaw)
|
|
return result
|
|
|
|
|
|
class TypeEnum(Enum):
|
|
ASSET = "asset"
|
|
LIGHT = "light"
|
|
|
|
|
|
@dataclass
|
|
class Instance:
|
|
model_name: Optional[str] = None
|
|
model_id: Optional[str] = None
|
|
id: Optional[str] = None
|
|
pose: Optional[Pose] = None
|
|
scale: Optional[int] = None
|
|
type: Optional[TypeEnum] = None
|
|
parent: Optional[str] = None
|
|
light_type: Optional[str] = None
|
|
intencity: Optional[int] = None
|
|
diffuse: Optional[List[float]] = None
|
|
spot_angle: Optional[int] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "Instance":
|
|
assert isinstance(obj, dict)
|
|
model_name = from_union([from_str, from_none], obj.get("model_name"))
|
|
model_id = from_union([from_str, from_none], obj.get("model_id"))
|
|
id = from_union([from_str, from_none], obj.get("id"))
|
|
pose = from_union([Pose.from_dict, from_none], obj.get("pose"))
|
|
scale = from_union([from_int, from_none], obj.get("scale"))
|
|
type = from_union([TypeEnum, from_none], obj.get("type"))
|
|
parent = from_union([from_str, from_none], obj.get("parent"))
|
|
light_type = from_union([from_str, from_none], obj.get("light_type"))
|
|
intencity = from_union([from_int, from_none], obj.get("intencity"))
|
|
diffuse = from_union(
|
|
[lambda x: from_list(from_float, x), from_none], obj.get("diffuse")
|
|
)
|
|
spot_angle = from_union([from_int, from_none], obj.get("spot_angle"))
|
|
return Instance(
|
|
model_name,
|
|
model_id,
|
|
id,
|
|
pose,
|
|
scale,
|
|
type,
|
|
parent,
|
|
light_type,
|
|
intencity,
|
|
diffuse,
|
|
spot_angle,
|
|
)
|
|
|
|
def fromMappingInstanceAtModel(
|
|
self, models: List[Model]
|
|
) -> "MappingInstanceAtModel":
|
|
for el in models:
|
|
if el.id == self.model_id:
|
|
return MappingInstanceAtModel(instance=self, model=el)
|
|
return Failure("not found model at {self.model_id} ")
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.model_name is not None:
|
|
result["model_name"] = from_union([from_str, from_none], self.model_name)
|
|
if self.model_id is not None:
|
|
result["model_id"] = from_union([from_str, from_none], self.model_id)
|
|
if self.id is not None:
|
|
result["id"] = from_union([from_str, from_none], self.id)
|
|
if self.pose is not None:
|
|
result["pose"] = from_union(
|
|
[lambda x: to_class(Pose, x), from_none], self.pose
|
|
)
|
|
if self.scale is not None:
|
|
result["scale"] = from_union([from_int, from_none], self.scale)
|
|
if self.type is not None:
|
|
result["type"] = from_union(
|
|
[lambda x: to_enum(TypeEnum, x), from_none], self.type
|
|
)
|
|
if self.parent is not None:
|
|
result["parent"] = from_union([from_str, from_none], self.parent)
|
|
if self.light_type is not None:
|
|
result["light_type"] = from_union([from_str, from_none], self.light_type)
|
|
if self.intencity is not None:
|
|
result["intencity"] = from_union([from_int, from_none], self.intencity)
|
|
if self.diffuse is not None:
|
|
result["diffuse"] = from_union(
|
|
[lambda x: from_list(to_float, x), from_none], self.diffuse
|
|
)
|
|
if self.spot_angle is not None:
|
|
result["spot_angle"] = from_union([from_int, from_none], self.spot_angle)
|
|
return result
|
|
|
|
|
|
class BasePose:
|
|
def __init__(self, x: float, y: float, z: float, **kwargs):
|
|
self.x = x
|
|
self.y = y
|
|
self.z = z
|
|
|
|
def toPose(self, sdfXmlMock: str):
|
|
return (
|
|
sdfXmlMock.replace("{x}", str(self.x))
|
|
.replace("{y}", str(self.y))
|
|
.replace("{z}", str(self.z))
|
|
)
|
|
|
|
|
|
class MappingInstanceAtModel(BasePose):
|
|
instance: Instance
|
|
model: Model
|
|
|
|
def __init__(self, instance: Instance, model: Model) -> None:
|
|
self.instance = instance
|
|
self.model = model
|
|
pass
|
|
|
|
def toSDF(self):
|
|
pose = self.instance.pose
|
|
match self.instance.type:
|
|
case TypeEnum.ASSET:
|
|
mock = FileSystemRepository.readFile(
|
|
os.path.dirname(os.path.realpath(__file__))
|
|
+ "/../mocks/model_include_sdf.xml"
|
|
)
|
|
# mockPose = self.toPose(mock)
|
|
return (
|
|
mock.replace("{name}", str(self.model.name))
|
|
.replace("{x}", str(pose.x))
|
|
.replace("{y}", str(pose.y))
|
|
.replace("{z}", str(pose.z))
|
|
.replace("{pitch}", str(pose.pitch))
|
|
.replace("{yaw}", str(pose.yaw))
|
|
.replace("{roll}", str(pose.roll))
|
|
.replace("{uri}", str(self.model.path))
|
|
)
|
|
case TypeEnum.LIGHT:
|
|
pathMock = (
|
|
os.path.dirname(os.path.realpath(__file__))
|
|
+ "/../mocks/light_sdf.xml"
|
|
)
|
|
|
|
return (
|
|
FileSystemRepository.readFile(pathMock)
|
|
.replace("{x}", str(pose.x))
|
|
.replace("{y}", str(pose.y))
|
|
.replace("{z}", str(pose.z))
|
|
.replace("{pitch}", str(pose.pitch))
|
|
.replace("{yaw}", str(pose.yaw))
|
|
.replace("{roll}", str(pose.roll))
|
|
.replace("{type_light}", str(self.instance.light_type))
|
|
.replace("{name_light}", str("132"))
|
|
.replace("{r}", self.instance.diffuse[0])
|
|
.replace("{g}", self.instance.diffuse[1])
|
|
.replace("{b}", self.instance.diffuse[2])
|
|
.replace("{a}", self.instance.diffuse[3])
|
|
)
|
|
|
|
|
|
@dataclass
|
|
class Gravity:
|
|
x: Optional[int] = None
|
|
y: Optional[int] = None
|
|
z: Optional[float] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "Gravity":
|
|
assert isinstance(obj, dict)
|
|
x = from_union([from_int, from_none], obj.get("x"))
|
|
y = from_union([from_int, from_none], obj.get("y"))
|
|
z = from_union([from_float, from_none], obj.get("z"))
|
|
return Gravity(x, y, z)
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.x is not None:
|
|
result["x"] = from_union([from_int, from_none], self.x)
|
|
if self.y is not None:
|
|
result["y"] = from_union([from_int, from_none], self.y)
|
|
if self.z is not None:
|
|
result["z"] = from_union([to_float, from_none], self.z)
|
|
return result
|
|
|
|
|
|
@dataclass
|
|
class Physics:
|
|
engine_name: Optional[str] = None
|
|
gravity: Optional[Gravity] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "Physics":
|
|
assert isinstance(obj, dict)
|
|
engine_name = from_union([from_str, from_none], obj.get("engine_name"))
|
|
gravity = from_union([Gravity.from_dict, from_none], obj.get("gravity"))
|
|
return Physics(engine_name, gravity)
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.engine_name is not None:
|
|
result["engine_name"] = from_union([from_str, from_none], self.engine_name)
|
|
if self.gravity is not None:
|
|
result["gravity"] = from_union(
|
|
[lambda x: to_class(Gravity, x), from_none], self.gravity
|
|
)
|
|
return result
|
|
|
|
def toSDF(self) -> str:
|
|
pathMock = os.path.dirname(os.path.realpath(__file__)) + "/../mocks/world.xml"
|
|
gravity = self.gravity
|
|
|
|
return (
|
|
FileSystemRepository.readFile(pathMock)
|
|
.replace("{gravity_x}", str(gravity.x))
|
|
.replace("{gravity_y}", str(gravity.y))
|
|
.replace("{gravity_z}", str(gravity.z))
|
|
.replace("{engine_type}", str(self.engine_name))
|
|
)
|
|
|
|
|
|
@dataclass
|
|
class RobossemblerAssets:
|
|
models: Optional[List[Model]] = None
|
|
instances: Optional[List[Instance]] = None
|
|
physics: Optional[Physics] = None
|
|
|
|
@staticmethod
|
|
def from_dict(obj: Any) -> "RobossemblerAssets":
|
|
assert isinstance(obj, dict)
|
|
models = from_union(
|
|
[lambda x: from_list(Model.from_dict, x), from_none], obj.get("models")
|
|
)
|
|
|
|
instances = from_union(
|
|
[lambda x: from_list(Instance.from_dict, x), from_none],
|
|
obj.get("instances"),
|
|
)
|
|
|
|
physics = from_union([Physics.from_dict, from_none], obj.get("physics"))
|
|
return RobossemblerAssets(models, instances, physics)
|
|
|
|
def to_dict(self) -> dict:
|
|
result: dict = {}
|
|
if self.models is not None:
|
|
result["models"] = from_union(
|
|
[lambda x: from_list(lambda x: to_class(Model, x), x), from_none],
|
|
self.models,
|
|
)
|
|
if self.instances is not None:
|
|
result["instances"] = from_union(
|
|
[lambda x: from_list(lambda x: to_class(Instance, x), x), from_none],
|
|
self.instances,
|
|
)
|
|
if self.physics is not None:
|
|
result["physics"] = from_union(
|
|
[lambda x: to_class(Physics, x), from_none], self.physics
|
|
)
|
|
return result
|
|
|
|
def _getAllAtType(self, type: TypeEnum) -> List[Instance]:
|
|
return list(filter(lambda x: x.type == type, self.instances))
|
|
|
|
def getAllLightInstances(self) -> List[Instance]:
|
|
return list(
|
|
map(
|
|
lambda el: el.fromMappingInstanceAtModel(self.models),
|
|
self._getAllAtType(type=TypeEnum.LIGHT),
|
|
)
|
|
)
|
|
|
|
def getAllAssetsInstanceAtModel(self) -> List[MappingInstanceAtModel]:
|
|
return list(
|
|
map(
|
|
lambda el: el.fromMappingInstanceAtModel(self.models),
|
|
self._getAllAtType(type=TypeEnum.ASSET),
|
|
)
|
|
)
|