Skip to content

Commit

Permalink
trampoline classes for abstract c++ classes
Browse files Browse the repository at this point in the history
  • Loading branch information
@DimasfromLavoisier
DimasfromLavoisier committed Jul 31, 2024
1 parent d0a5b65 commit 3b4c4e1
Show file tree
Hide file tree
Showing 21 changed files with 1,827 additions and 918 deletions.
18 changes: 9 additions & 9 deletions python/helios/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
from helios.leg import Leg
from helios.platformsettings import PlatformSettings
from helios.scannersettings import ScannerSettings
from helios.platform import Platform
from helios.scanner import Scanner
from helios.survey import Survey
from helios.scenepart import ScenePart
from helios.scene import Scene
from helios.util import * # change this to import only the necessary functions later
# from helios.leg import Leg
# from helios.platformsettings import PlatformSettings
# from helios.scannersettings import ScannerSettings
# from helios.platform import Platform
# from helios.scanner import Scanner
# from helios.survey import Survey
# from helios.scenepart import ScenePart
# from helios.scene import Scene
# #from helios.util import * # change this to import only the necessary functions later
46 changes: 23 additions & 23 deletions python/helios/leg.py
View file
Original file line number Diff line number Diff line change
@@ -1,26 +1,26 @@
from pydantic import BaseModel, Field, NonNegativeInt
from typing import Optional, Union
from helios.scannersettings import ScannerSettings
from helios.platformsettings import PlatformSettings
# from pydantic import BaseModel, Field, NonNegativeInt
# from typing import Optional, Union
# from helios.scannersettings import ScannerSettings
# from helios.platformsettings import PlatformSettings

import numpy as np
# import numpy as np

class Leg(BaseModel):
scanner_settings: Optional[ScannerSettings] = Field(default=None)
platform_settings: Optional[PlatformSettings] = Field(default=None)
trajectory_settings: Optional[TrajectorySettings] = Field(default=None)
length: float = Field(default=0.0)
serial_id: NonNegativeInt
strip: Optional[ScanningStrip] = Field(default=None)
was_processed: bool = Field(default=False)
look_at: Optional[Union[np.ndarray, int]] = Field(default=None) # Only for TLS!! Might transfer to utils
position: Optional[np.ndarray] = Field(default=None)
horizontal_fov: Optional[float] = Field(default=None)
vertical_fov: Optional[float] = Field(default=None)
horizontal_resolution: Optional[float] = Field(default=None)
vertical_resolution: Optional[float] = Field(default=None)
is_active: bool = Field(default=True)
# class Leg(BaseModel):
# scanner_settings: Optional[ScannerSettings] = Field(default=None)
# platform_settings: Optional[PlatformSettings] = Field(default=None)
# trajectory_settings: Optional[TrajectorySettings] = Field(default=None)
# length: float = Field(default=0.0)
# serial_id: NonNegativeInt
# strip: Optional[ScanningStrip] = Field(default=None)
# was_processed: bool = Field(default=False)
# look_at: Optional[Union[np.ndarray, int]] = Field(default=None) # Only for TLS!! Might transfer to utils
# position: Optional[np.ndarray] = Field(default=None)
# horizontal_fov: Optional[float] = Field(default=None)
# vertical_fov: Optional[float] = Field(default=None)
# horizontal_resolution: Optional[float] = Field(default=None)
# vertical_resolution: Optional[float] = Field(default=None)
# is_active: bool = Field(default=True)

@property
def belongs_to_strip(self) -> bool:
return self.strip is not None
# @property
# def belongs_to_strip(self) -> bool:
# return self.strip is not None
312 changes: 156 additions & 156 deletions python/helios/platform.py
View file
Original file line number Diff line number Diff line change
@@ -1,169 +1,169 @@

from typing import Optional
from pydantic import BaseModel
import numpy as np
import math
from helios.scene import Scene
from helios.platformsettings import PlatformSettings

class Platform(BaseModel):
device_relative_position: np.ndarray = np.array([0.0, 0.0, 0.0])
device_relative_attitude: Rotation = Rotation()
scene: Optional[Scene] = None

position_x_noise_source: Optional[NoiseSource] = None
position_y_noise_source: Optional[NoiseSource] = None
position_z_noise_source: Optional[NoiseSource] = None
attitude_x_noise_source: Optional[NoiseSource] = None
attitude_y_noise_source: Optional[NoiseSource] = None
attitude_z_noise_source: Optional[NoiseSource] = None

settings_speed_m_s: float = 0.0
_origin_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
_target_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
_next_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
is_on_ground: bool = False
is_stop_and_turn: bool = True
is_smooth_turn: bool = False
is_slowdown_enabled: bool = True

_position: np.ndarray = np.array([0.0, 0.0, 0.0])
_attitude: Rotation = Rotation()
#caches
cached_absolute_mount_position: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_absolute_mount_attitude: Rotation = Rotation()
cached_dir_current: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_dir_current_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_array_to_target: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_array_to_target_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_distance_to_target_xy: float = 0.0
cached_origin_to_target_dir_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_target_to_next_dir_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
cached_end_target_angle_xy: float = 0.0
cached_current_angle_xy: float = 0.0
cached_origin_to_target_angle_xy: float = 0.0
cached_target_to_next_angle_xy: float = 0.0


#Проверить нужны ли эти декораторы во всех файлах!!!!!!
@property
def absolute_mount_attitude(self) -> Rotation:
return self.cached_absolute_mount_attitude

@property
def absolute_mount_position(self) -> np.ndarray:
return self.cached_absolute_mount_position
# from typing import Optional
# from pydantic import BaseModel
# import numpy as np
# import math
# from helios.scene import Scene
# from helios.platformsettings import PlatformSettings

# class Platform(BaseModel):
# device_relative_position: np.ndarray = np.array([0.0, 0.0, 0.0])
# device_relative_attitude: Rotation = Rotation()
# scene: Optional[Scene] = None

# position_x_noise_source: Optional[NoiseSource] = None
# position_y_noise_source: Optional[NoiseSource] = None
# position_z_noise_source: Optional[NoiseSource] = None
# attitude_x_noise_source: Optional[NoiseSource] = None
# attitude_y_noise_source: Optional[NoiseSource] = None
# attitude_z_noise_source: Optional[NoiseSource] = None

# settings_speed_m_s: float = 0.0
# _origin_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
# _target_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
# _next_waypoint: np.ndarray = np.array([0.0, 0.0, 0.0])
# is_on_ground: bool = False
# is_stop_and_turn: bool = True
# is_smooth_turn: bool = False
# is_slowdown_enabled: bool = True

# _position: np.ndarray = np.array([0.0, 0.0, 0.0])
# _attitude: Rotation = Rotation()
# #caches
# cached_absolute_mount_position: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_absolute_mount_attitude: Rotation = Rotation()
# cached_dir_current: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_dir_current_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_array_to_target: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_array_to_target_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_distance_to_target_xy: float = 0.0
# cached_origin_to_target_dir_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_target_to_next_dir_xy: np.ndarray = np.array([0.0, 0.0, 0.0])
# cached_end_target_angle_xy: float = 0.0
# cached_current_angle_xy: float = 0.0
# cached_origin_to_target_angle_xy: float = 0.0
# cached_target_to_next_angle_xy: float = 0.0


# #Проверить нужны ли эти декораторы во всех файлах!!!!!!
# @property
# def absolute_mount_attitude(self) -> Rotation:
# return self.cached_absolute_mount_attitude

# @property
# def absolute_mount_position(self) -> np.ndarray:
# return self.cached_absolute_mount_position

@property
def attitude(self) -> Rotation:
return self._attitude

@attitude.setter
def attitude(self, value: Rotation):
self._attitude = value
self.cached_absolute_mount_attitude = self._attitude.Rotation.apply_to(self.device_relative_attitude)

@property
def position(self) -> np.ndarray:
return self._position
# @property
# def attitude(self) -> Rotation:
# return self._attitude

# @attitude.setter
# def attitude(self, value: Rotation):
# self._attitude = value
# self.cached_absolute_mount_attitude = self._attitude.Rotation.apply_to(self.device_relative_attitude)

# @property
# def position(self) -> np.ndarray:
# return self._position

@position.setter
def position(self, value: np.ndarray):
self._position = value
self.cached_absolute_mount_position = self._position + self.device_relative_position
self.update_dynamic_cache()

@property
def origin_waypoint(self) -> np.ndarray:
return self._origin_waypoint
# @position.setter
# def position(self, value: np.ndarray):
# self._position = value
# self.cached_absolute_mount_position = self._position + self.device_relative_position
# self.update_dynamic_cache()

# @property
# def origin_waypoint(self) -> np.ndarray:
# return self._origin_waypoint

@origin_waypoint.setter
def origin_waypoint(self, value: np.ndarray):
self._origin_waypoint = value
self.update_static_cache()
# @origin_waypoint.setter
# def origin_waypoint(self, value: np.ndarray):
# self._origin_waypoint = value
# self.update_static_cache()

@property
def target_waypoint(self) -> np.ndarray:
return self._target_waypoint
# @property
# def target_waypoint(self) -> np.ndarray:
# return self._target_waypoint

@target_waypoint.setter
def target_waypoint(self, value: np.ndarray):
self._target_waypoint = value
self.update_static_cache()

@property
def next_waypoint(self) -> np.ndarray:
return self._next_waypoint
# @target_waypoint.setter
# def target_waypoint(self, value: np.ndarray):
# self._target_waypoint = value
# self.update_static_cache()

# @property
# def next_waypoint(self) -> np.ndarray:
# return self._next_waypoint

@next_waypoint.setter
def next_waypoint(self, value: np.ndarray):
self._next_waypoint = value
self.update_static_cache()

@property
def current_settings(self) -> PlatformSettings:
current_settings = PlatformSettings()
current_settings.speed_m_s = self.settings_speed_m_s
current_settings.is_on_ground = self.is_on_ground
current_settings.position(self.position)
return self.current_settings
# @next_waypoint.setter
# def next_waypoint(self, value: np.ndarray):
# self._next_waypoint = value
# self.update_static_cache()

# @property
# def current_settings(self) -> PlatformSettings:
# current_settings = PlatformSettings()
# current_settings.speed_m_s = self.settings_speed_m_s
# current_settings.is_on_ground = self.is_on_ground
# current_settings.position(self.position)
# return self.current_settings

def apply_settings(self, settings: PlatformSettings) -> None:
self.settings_speed_m_s = settings.speed_m_s
self.is_on_ground = settings.is_on_ground
self.position = settings.position
# def apply_settings(self, settings: PlatformSettings) -> None:
# self.settings_speed_m_s = settings.speed_m_s
# self.is_on_ground = settings.is_on_ground
# self.position = settings.position

@property
def current_direction(self) -> np.ndarray:
return self.cached_current_direction
# @property
# def current_direction(self) -> np.ndarray:
# return self.cached_current_direction

@property
def is_interpolated(self) -> bool:
return self.cached_is_interpolating
# @property
# def is_interpolated(self) -> bool:
# return self.cached_is_interpolating


def update_static_cache(self) -> None:
self.cached_origin_to_target_dir_xy = np.array([
self.target_waypoint[0] - self.origin_waypoint[0],
self.target_waypoint[1] - self.origin_waypoint[1],
0.0
])
self.cached_target_to_next_dir_xy = np.array([
self.next_waypoint[0] - self.target_waypoint[0],
self.next_waypoint[1] - self.target_waypoint[1],
0.0
])
self.cached_end_target_angle_xy = np.arccos(
np.dot(self.cached_origin_to_target_dir_xy, self.cached_target_to_next_dir_xy) /
(np.linalg.norm(self.cached_origin_to_target_dir_xy) * np.linalg.norm(self.cached_target_to_next_dir_xy))
)
if np.isnan(self.cached_end_target_angle_xy):
self.cached_end_target_angle_xy = 0.0
self.cached_origin_to_target_angle_xy = self.direction_to_angle_xy(self.cached_origin_to_target_dir_xy, True) # this function should be moved to a separate class
self.cached_target_to_next_angle_xy = self.direction_to_angle_xy(self.cached_target_to_next_dir_xy, True) # this function should be moved to a separate class
self.update_dynamic_cache()

def update_dynamic_cache(self) -> None:
self.cached_array_to_target = self.target_waypoint - self.position
self.cached_array_to_target_xy = np.array([self.cached_array_to_target[0], self.cached_array_to_target[1], 0.0])
self.cached_distance_to_target_xy = np.linalg.norm(self.cached_array_to_target_xy)
self.cached_dir_current = self.current_direction()
self.cached_dir_current_xy = np.array([self.cached_dir_current[0], self.cached_dir_current[1], 0.0])
self.cached_current_angle_xy = np.arccos(
np.dot(self.cached_dir_current_xy, self.cached_target_to_next_dir_xy) /
(np.linalg.norm(self.cached_dir_current_xy) * np.linalg.norm(self.cached_target_to_next_dir_xy))
)
if np.isnan(self.cached_current_angle_xy):
self.cached_current_angle_xy = 0.0


def direction_to_angle_xy(self, direction: np.ndarray, is_normalized: bool) -> float: # this function should be moved to a separate class
angle = np.arctan2(direction[0], direction[1])
if is_normalized and angle < 0.0:
angle += np.pi * 2
return angle
# def update_static_cache(self) -> None:
# self.cached_origin_to_target_dir_xy = np.array([
# self.target_waypoint[0] - self.origin_waypoint[0],
# self.target_waypoint[1] - self.origin_waypoint[1],
# 0.0
# ])
# self.cached_target_to_next_dir_xy = np.array([
# self.next_waypoint[0] - self.target_waypoint[0],
# self.next_waypoint[1] - self.target_waypoint[1],
# 0.0
# ])
# self.cached_end_target_angle_xy = np.arccos(
# np.dot(self.cached_origin_to_target_dir_xy, self.cached_target_to_next_dir_xy) /
# (np.linalg.norm(self.cached_origin_to_target_dir_xy) * np.linalg.norm(self.cached_target_to_next_dir_xy))
# )
# if np.isnan(self.cached_end_target_angle_xy):
# self.cached_end_target_angle_xy = 0.0
# self.cached_origin_to_target_angle_xy = self.direction_to_angle_xy(self.cached_origin_to_target_dir_xy, True) # this function should be moved to a separate class
# self.cached_target_to_next_angle_xy = self.direction_to_angle_xy(self.cached_target_to_next_dir_xy, True) # this function should be moved to a separate class
# self.update_dynamic_cache()

# def update_dynamic_cache(self) -> None:
# self.cached_array_to_target = self.target_waypoint - self.position
# self.cached_array_to_target_xy = np.array([self.cached_array_to_target[0], self.cached_array_to_target[1], 0.0])
# self.cached_distance_to_target_xy = np.linalg.norm(self.cached_array_to_target_xy)
# self.cached_dir_current = self.current_direction()
# self.cached_dir_current_xy = np.array([self.cached_dir_current[0], self.cached_dir_current[1], 0.0])
# self.cached_current_angle_xy = np.arccos(
# np.dot(self.cached_dir_current_xy, self.cached_target_to_next_dir_xy) /
# (np.linalg.norm(self.cached_dir_current_xy) * np.linalg.norm(self.cached_target_to_next_dir_xy))
# )
# if np.isnan(self.cached_current_angle_xy):
# self.cached_current_angle_xy = 0.0


# def direction_to_angle_xy(self, direction: np.ndarray, is_normalized: bool) -> float: # this function should be moved to a separate class
# angle = np.arctan2(direction[0], direction[1])
# if is_normalized and angle < 0.0:
# angle += np.pi * 2
# return angle

def get_by_name(self, name: str) -> Optional['Platform']:
if self.name == name:
return self
return None
# def get_by_name(self, name: str) -> Optional['Platform']:
# if self.name == name:
# return self
# return None
Loading

0 comments on commit 3b4c4e1

Please sign in to comment.