plexus-python-common 1.0.61__tar.gz → 1.0.63__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (93) hide show
  1. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/PKG-INFO +1 -1
  2. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/carto/OSMFile.py +1 -1
  3. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/carto/OSMNode.py +1 -1
  4. plexus_python_common-1.0.61/src/plexus/common/proj.py → plexus_python_common-1.0.63/src/plexus/common/utils/gisutils.py +102 -1
  5. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/tagutils.py +5 -5
  6. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/PKG-INFO +1 -1
  7. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/SOURCES.txt +2 -4
  8. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/carto/osm_file_test.py +2 -2
  9. plexus_python_common-1.0.61/test/plexus_tests/common/proj_test.py → plexus_python_common-1.0.63/test/plexus_tests/common/utils/gisutils_test.py +64 -2
  10. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/tagutils_test.py +26 -0
  11. plexus_python_common-1.0.61/src/plexus/common/pose.py +0 -107
  12. plexus_python_common-1.0.61/test/plexus_tests/common/pose_test.py +0 -66
  13. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/.editorconfig +0 -0
  14. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/.github/workflows/pr.yml +0 -0
  15. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/.github/workflows/push.yml +0 -0
  16. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/.gitignore +0 -0
  17. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/MANIFEST.in +0 -0
  18. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/README.md +0 -0
  19. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/VERSION +0 -0
  20. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/pyproject.toml +0 -0
  21. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/jsonutils/dummy.0.jsonl +0 -0
  22. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/jsonutils/dummy.1.jsonl +0 -0
  23. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/jsonutils/dummy.2.jsonl +0 -0
  24. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/0-dummy +0 -0
  25. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/1-dummy +0 -0
  26. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/2-dummy +0 -0
  27. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.0.0.jsonl +0 -0
  28. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.0.0.vol-0.jsonl +0 -0
  29. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.0.jsonl +0 -0
  30. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.1.1.jsonl +0 -0
  31. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.1.1.vol-1.jsonl +0 -0
  32. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.1.jsonl +0 -0
  33. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.2.2.jsonl +0 -0
  34. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.2.2.vol-2.jsonl +0 -0
  35. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.2.jsonl +0 -0
  36. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.csv.part0 +0 -0
  37. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.csv.part1 +0 -0
  38. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.csv.part2 +0 -0
  39. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/pathutils/dummy.txt +0 -0
  40. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.baz/file.bar.baz +0 -0
  41. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.baz/file.foo.bar +0 -0
  42. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.baz/file.foo.baz +0 -0
  43. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/dir.foo.bar/dir.foo.bar.baz/file.foo.bar.baz +0 -0
  44. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/dir.foo.bar/file.bar.baz +0 -0
  45. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/dir.foo.bar/file.foo.bar +0 -0
  46. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/dir.foo.bar/file.foo.baz +0 -0
  47. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/file.bar +0 -0
  48. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/file.baz +0 -0
  49. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils/dir.foo/file.foo +0 -0
  50. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils_archive/archive.compressed.zip +0 -0
  51. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/resources/unittest/s3utils_archive/archive.uncompressed.zip +0 -0
  52. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/setup.cfg +0 -0
  53. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/setup.py +0 -0
  54. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/__init__.py +0 -0
  55. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/carto/OSMTags.py +0 -0
  56. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/carto/OSMWay.py +0 -0
  57. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/carto/__init__.py +0 -0
  58. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/resources/__init__.py +0 -0
  59. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/resources/tags/__init__.py +0 -0
  60. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/resources/tags/unittest-1.0.0.tagset.yaml +0 -0
  61. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/resources/tags/universal-1.0.0.tagset.yaml +0 -0
  62. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/__init__.py +0 -0
  63. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/apiutils.py +0 -0
  64. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/bagutils.py +0 -0
  65. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/config.py +0 -0
  66. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/datautils.py +0 -0
  67. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/dockerutils.py +0 -0
  68. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/jsonutils.py +0 -0
  69. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/ormutils.py +0 -0
  70. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/pathutils.py +0 -0
  71. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/s3utils.py +0 -0
  72. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/sqlutils.py +0 -0
  73. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/strutils.py +0 -0
  74. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus/common/utils/testutils.py +0 -0
  75. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/dependency_links.txt +0 -0
  76. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/not-zip-safe +0 -0
  77. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/requires.txt +0 -0
  78. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/src/plexus_python_common.egg-info/top_level.txt +0 -0
  79. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/__init__.py +0 -0
  80. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/__init__.py +0 -0
  81. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/carto/__init__.py +0 -0
  82. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/carto/osm_tags_test.py +0 -0
  83. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/__init__.py +0 -0
  84. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/bagutils_test.py +0 -0
  85. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/datautils_test.py +0 -0
  86. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/dockerutils_test.py +0 -0
  87. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/jsonutils_test.py +0 -0
  88. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/ormutils_test.py +0 -0
  89. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/pathutils_test.py +0 -0
  90. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/s3utils_test.py +0 -0
  91. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/strutils_test.py +0 -0
  92. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/plexus_tests/common/utils/testutils_test.py +0 -0
  93. {plexus_python_common-1.0.61 → plexus_python_common-1.0.63}/test/testenv.py +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: plexus-python-common
3
- Version: 1.0.61
3
+ Version: 1.0.63
4
4
  Classifier: Programming Language :: Python :: 3
5
5
  Classifier: Programming Language :: Python :: 3.12
6
6
  Classifier: Programming Language :: Python :: 3.13
@@ -5,7 +5,7 @@ import lxml.etree
5
5
  from plexus.common.carto.OSMNode import OSMNode
6
6
  from plexus.common.carto.OSMTags import OSMTags
7
7
  from plexus.common.carto.OSMWay import OSMWay
8
- from plexus.common.proj import Coord, Proj
8
+ from plexus.common.utils.gisutils import Coord, Proj
9
9
 
10
10
 
11
11
  class OSMFile(object):
@@ -1,7 +1,7 @@
1
1
  from iker.common.utils.strutils import repr_data
2
2
 
3
3
  from plexus.common.carto.OSMTags import OSMTags
4
- from plexus.common.proj import Coord
4
+ from plexus.common.utils.gisutils import Coord
5
5
 
6
6
 
7
7
  class OSMNode(object):
@@ -5,10 +5,111 @@ from typing import Annotated, Self
5
5
  import numpy as np
6
6
  import pydantic as pdt
7
7
  import pyproj
8
+ import pyquaternion as pyquat
8
9
  from iker.common.utils.funcutils import singleton
9
10
  from iker.common.utils.strutils import parse_params_string, repr_data, str_conv
10
11
 
11
- from plexus.common.pose import Pose
12
+
13
+ class Pose(object):
14
+ @classmethod
15
+ def from_numbers(
16
+ cls,
17
+ px: float,
18
+ py: float,
19
+ pz: float,
20
+ qx: float,
21
+ qy: float,
22
+ qz: float,
23
+ qw: float,
24
+ ts: float = 0,
25
+ ) -> Self:
26
+ """
27
+ Constructs a pose from numbers representing position and orientation
28
+ """
29
+ return Pose(ts, np.array([px, py, pz], dtype=np.float64), np.array([qw, qx, qy, qz], dtype=np.float64))
30
+
31
+ @classmethod
32
+ def add(cls, x: Self, d: Self) -> Self:
33
+ """
34
+ Performs pose SE3 addition, as x + d = y
35
+ """
36
+ xq = pyquat.Quaternion(x.q)
37
+ dq = pyquat.Quaternion(d.q)
38
+
39
+ yp = x.p + xq.rotate(d.p)
40
+ yq = xq * dq
41
+
42
+ return Pose(0, yp, yq.normalised.elements)
43
+
44
+ @classmethod
45
+ def sub(cls, y: Self, x: Self) -> Self:
46
+ """
47
+ Performs pose SE3 subtraction, as x + d = y => d = y - x
48
+ """
49
+ xq = pyquat.Quaternion(x.q)
50
+ yq = pyquat.Quaternion(y.q)
51
+
52
+ dp = xq.inverse.rotate(y.p - x.p)
53
+ dq = xq.inverse * yq
54
+
55
+ return Pose(0, dp, dq.normalised.elements)
56
+
57
+ @classmethod
58
+ def interpolate(cls, a: Self, b: Self, t: float) -> Self:
59
+ """
60
+ Interpolates between two given poses, as a * t + b * (1 - t)
61
+
62
+ :return: interpolated pose
63
+ """
64
+ ts = a.ts + (b.ts - a.ts) * t
65
+ p = a.p + (b.p - a.p) * t
66
+ q = pyquat.Quaternion.slerp(pyquat.Quaternion(a.q), pyquat.Quaternion(b.q), t)
67
+ return Pose(ts, p, q.normalised.elements)
68
+
69
+ def __init__(self, ts: float, p: np.ndarray, q: np.ndarray):
70
+ """
71
+ Represents a pose
72
+
73
+ :param ts: timestamp
74
+ :param p: position vector
75
+ :param q: orientation quaternion
76
+ """
77
+ self.ts = ts
78
+ self.p = p
79
+ self.q = q
80
+
81
+ def matrix(self) -> np.ndarray:
82
+ """
83
+ Returns the transformation matrix of this pose
84
+
85
+ :return: transformation matrix
86
+ """
87
+ r = pyquat.Quaternion(self.q).rotation_matrix
88
+ t = self.p[:, None]
89
+ return np.block([[r, t], [np.zeros((1, 3), dtype=np.float64), np.ones((1, 1), dtype=np.float64)]])
90
+
91
+ def translate(self, v: np.ndarray) -> np.ndarray:
92
+ """
93
+ Translate the given vector with the pose position
94
+
95
+ :param v: the vector to be translated
96
+
97
+ :return: translated vector
98
+ """
99
+ return self.p + v
100
+
101
+ def rotate(self, v: np.ndarray) -> np.ndarray:
102
+ """
103
+ Rotates the given vector with the pose orientation
104
+
105
+ :param v: the vector to be rotated
106
+
107
+ :return: rotated vector
108
+ """
109
+ return pyquat.Quaternion(self.q).rotate(v)
110
+
111
+ def __str__(self):
112
+ return repr_data(self)
12
113
 
13
114
 
14
115
  class Proj(ABC):
@@ -382,11 +382,11 @@ class TagTarget(BaseModel):
382
382
  description="Name of the tagger that generates the tag records for the target",
383
383
  )
384
384
  tagger_version: str = Field(
385
- sa_column=sa.Column(sa_sqlite.VARCHAR(64), nullable=False),
385
+ sa_column=sa.Column(sa_sqlite.VARCHAR(32), nullable=False),
386
386
  description="Version of the tagger that generates the tag records for the target",
387
387
  )
388
388
  vehicle_name: str = Field(
389
- sa_column=sa.Column(sa_sqlite.TEXT, nullable=False),
389
+ sa_column=sa.Column(sa_sqlite.VARCHAR(128), nullable=False),
390
390
  description="Vehicle name associated with the tag record",
391
391
  )
392
392
  begin_dt: datetime.datetime = Field(
@@ -455,12 +455,12 @@ class TagRecord(BaseModel):
455
455
  description="End datetime of the tag record",
456
456
  )
457
457
  tagset_namespace: str | None = Field(
458
- sa_column=sa.Column(sa_sqlite.VARCHAR(128), nullable=True),
458
+ sa_column=sa.Column(sa_sqlite.VARCHAR(64), nullable=True),
459
459
  default=None,
460
460
  description="Namespace of the tagset that the tag belongs to",
461
461
  )
462
462
  tagset_version: str | None = Field(
463
- sa_column=sa.Column(sa_sqlite.VARCHAR(64), nullable=True),
463
+ sa_column=sa.Column(sa_sqlite.VARCHAR(32), nullable=True),
464
464
  default=None,
465
465
  description="Version of the tagset that the tag belongs to",
466
466
  )
@@ -469,7 +469,7 @@ class TagRecord(BaseModel):
469
469
  description="Tag name",
470
470
  )
471
471
  props: JsonType | None = Field(
472
- sa_column=sa.Column(sa_sqlite.TEXT, nullable=True),
472
+ sa_column=sa.Column(sa_sqlite.JSON, nullable=True),
473
473
  default=None,
474
474
  description="Additional properties of the tag record in JSON format",
475
475
  )
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: plexus-python-common
3
- Version: 1.0.61
3
+ Version: 1.0.63
4
4
  Classifier: Programming Language :: Python :: 3
5
5
  Classifier: Programming Language :: Python :: 3.12
6
6
  Classifier: Programming Language :: Python :: 3.13
@@ -39,8 +39,6 @@ resources/unittest/s3utils/dir.foo/dir.foo.bar/dir.foo.bar.baz/file.foo.bar.baz
39
39
  resources/unittest/s3utils_archive/archive.compressed.zip
40
40
  resources/unittest/s3utils_archive/archive.uncompressed.zip
41
41
  src/plexus/common/__init__.py
42
- src/plexus/common/pose.py
43
- src/plexus/common/proj.py
44
42
  src/plexus/common/carto/OSMFile.py
45
43
  src/plexus/common/carto/OSMNode.py
46
44
  src/plexus/common/carto/OSMTags.py
@@ -56,6 +54,7 @@ src/plexus/common/utils/bagutils.py
56
54
  src/plexus/common/utils/config.py
57
55
  src/plexus/common/utils/datautils.py
58
56
  src/plexus/common/utils/dockerutils.py
57
+ src/plexus/common/utils/gisutils.py
59
58
  src/plexus/common/utils/jsonutils.py
60
59
  src/plexus/common/utils/ormutils.py
61
60
  src/plexus/common/utils/pathutils.py
@@ -73,8 +72,6 @@ src/plexus_python_common.egg-info/top_level.txt
73
72
  test/testenv.py
74
73
  test/plexus_tests/__init__.py
75
74
  test/plexus_tests/common/__init__.py
76
- test/plexus_tests/common/pose_test.py
77
- test/plexus_tests/common/proj_test.py
78
75
  test/plexus_tests/common/carto/__init__.py
79
76
  test/plexus_tests/common/carto/osm_file_test.py
80
77
  test/plexus_tests/common/carto/osm_tags_test.py
@@ -82,6 +79,7 @@ test/plexus_tests/common/utils/__init__.py
82
79
  test/plexus_tests/common/utils/bagutils_test.py
83
80
  test/plexus_tests/common/utils/datautils_test.py
84
81
  test/plexus_tests/common/utils/dockerutils_test.py
82
+ test/plexus_tests/common/utils/gisutils_test.py
85
83
  test/plexus_tests/common/utils/jsonutils_test.py
86
84
  test/plexus_tests/common/utils/ormutils_test.py
87
85
  test/plexus_tests/common/utils/pathutils_test.py
@@ -4,8 +4,8 @@ import unittest.mock
4
4
  import lxml.etree
5
5
 
6
6
  from plexus.common.carto import OSMFile
7
- from plexus.common.proj import Coord
8
- from plexus.common.proj import make_proj
7
+ from plexus.common.utils.gisutils import Coord
8
+ from plexus.common.utils.gisutils import make_proj
9
9
 
10
10
 
11
11
  class MockedTree(object):
@@ -1,11 +1,73 @@
1
+ import math
1
2
  import random
2
3
  import unittest
3
4
 
4
5
  import ddt
6
+ import numpy as np
5
7
  import pydantic as pdt
8
+ import pyquaternion as pyquat
9
+ from iker.common.utils.randutils import randomizer
6
10
 
7
- from plexus.common.proj import Coord, EQDCProj, UTMProj, WebMercProj
8
- from plexus.common.proj import make_proj
11
+ from plexus.common.utils.gisutils import Coord, EQDCProj, UTMProj, WebMercProj
12
+ from plexus.common.utils.gisutils import Pose
13
+ from plexus.common.utils.gisutils import make_proj
14
+
15
+
16
+ @ddt.ddt
17
+ class PoseTest(unittest.TestCase):
18
+
19
+ @staticmethod
20
+ def random_pose() -> Pose:
21
+ q = pyquat.Quaternion(axis=randomizer().random_unit_vector(3),
22
+ angle=math.pi * randomizer().next_float(0.0, 0.5))
23
+ return Pose(0, np.array(randomizer().random_unit_vector(3)), q.normalised.elements)
24
+
25
+ def assert_array_almost_equal(self, xs: np.ndarray, ys: np.ndarray, delta: float):
26
+ self.assertEqual(len(xs), len(ys))
27
+ for x, y in zip(xs, ys):
28
+ self.assertAlmostEqual(x, y, delta=delta)
29
+
30
+ def test_builtin_init(self):
31
+ for _ in range(0, 10000):
32
+ expect = PoseTest.random_pose()
33
+ actual = Pose(0, expect.p, expect.q)
34
+
35
+ self.assert_array_almost_equal(expect.p, actual.p, delta=1e-9)
36
+ self.assert_array_almost_equal(expect.q, actual.q, delta=1e-9)
37
+
38
+ def test_add_sub(self):
39
+ for _ in range(0, 10000):
40
+ pose = PoseTest.random_pose()
41
+ delta = PoseTest.random_pose()
42
+
43
+ result = Pose.sub(Pose.add(pose, delta), pose)
44
+
45
+ self.assert_array_almost_equal(delta.p, result.p, delta=1e-9)
46
+ self.assert_array_almost_equal(delta.q, result.q, delta=1e-9)
47
+
48
+ def test_interpolate(self):
49
+ for _ in range(0, 10000):
50
+ t = randomizer().next_float()
51
+
52
+ pose1 = PoseTest.random_pose()
53
+ pose2 = PoseTest.random_pose()
54
+
55
+ inter1 = Pose.interpolate(pose1, pose2, t)
56
+ inter2 = Pose.interpolate(pose2, pose1, 1.0 - t)
57
+
58
+ self.assert_array_almost_equal(inter1.p, inter2.p, delta=1e-9)
59
+ self.assert_array_almost_equal(inter1.q, inter2.q, delta=1e-9)
60
+
61
+ def test_matrix(self):
62
+ for _ in range(0, 10000):
63
+ point = np.array(randomizer().random_unit_vector(3))
64
+
65
+ pose = PoseTest.random_pose()
66
+
67
+ expect = pose.translate(pose.rotate(point))
68
+ result = np.matmul(pose.matrix(), np.array([*point, 1.0])[:, None]).transpose()[0][:3]
69
+
70
+ self.assert_array_almost_equal(expect, result, delta=1e-9)
9
71
 
10
72
 
11
73
  @ddt.ddt
@@ -133,6 +133,19 @@ class TagUtilsTest(unittest.TestCase):
133
133
  dt_parse_iso("2020-01-01T00:00:00+00:00") + datetime.timedelta(seconds=i),
134
134
  dt_parse_iso("2020-01-02T00:00:00+00:00") + datetime.timedelta(seconds=i + 1),
135
135
  tag,
136
+ props={
137
+ "dummy_int": 1,
138
+ "dummy_float": 1.0e-1,
139
+ "dummy_bool": True,
140
+ "dummy_str": "dummy_string",
141
+ "dummy_list": [1, 1.0e-1, True, "dummy_string"],
142
+ "dummy_dict": {
143
+ "dummy_int": 1,
144
+ "dummy_float": 1.0e-1,
145
+ "dummy_bool": True,
146
+ "dummy_str": "dummy_string",
147
+ },
148
+ },
136
149
  )
137
150
 
138
151
  self.assertEqual(len(list(target_cache.iter_tags())), tag_records_count)
@@ -271,6 +284,19 @@ class TagUtilsTest(unittest.TestCase):
271
284
  dt_parse_iso("2020-01-01T00:00:00+00:00") + datetime.timedelta(seconds=i),
272
285
  dt_parse_iso("2020-01-02T00:00:00+00:00") + datetime.timedelta(seconds=i + 1),
273
286
  tag,
287
+ props={
288
+ "dummy_int": 1,
289
+ "dummy_float": 1.0e-1,
290
+ "dummy_bool": True,
291
+ "dummy_str": "dummy_string",
292
+ "dummy_list": [1, 1.0e-1, True, "dummy_string"],
293
+ "dummy_dict": {
294
+ "dummy_int": 1,
295
+ "dummy_float": 1.0e-1,
296
+ "dummy_bool": True,
297
+ "dummy_str": "dummy_string",
298
+ },
299
+ },
274
300
  )
275
301
 
276
302
  threads = [threading.Thread(target=worker, args=(tid,)) for tid in range(total_threads_count)]
@@ -1,107 +0,0 @@
1
- from typing import Self
2
-
3
- import numpy as np
4
- import pyquaternion as pyquat
5
- from iker.common.utils.strutils import repr_data
6
-
7
-
8
- class Pose(object):
9
- @classmethod
10
- def from_numbers(
11
- cls,
12
- px: float,
13
- py: float,
14
- pz: float,
15
- qx: float,
16
- qy: float,
17
- qz: float,
18
- qw: float,
19
- ts: float = 0,
20
- ) -> Self:
21
- """
22
- Constructs a pose from numbers representing position and orientation
23
- """
24
- return Pose(ts, np.array([px, py, pz], dtype=np.float64), np.array([qw, qx, qy, qz], dtype=np.float64))
25
-
26
- @classmethod
27
- def add(cls, x: Self, d: Self) -> Self:
28
- """
29
- Performs pose SE3 addition, as x + d = y
30
- """
31
- xq = pyquat.Quaternion(x.q)
32
- dq = pyquat.Quaternion(d.q)
33
-
34
- yp = x.p + xq.rotate(d.p)
35
- yq = xq * dq
36
-
37
- return Pose(0, yp, yq.normalised.elements)
38
-
39
- @classmethod
40
- def sub(cls, y: Self, x: Self) -> Self:
41
- """
42
- Performs pose SE3 subtraction, as x + d = y => d = y - x
43
- """
44
- xq = pyquat.Quaternion(x.q)
45
- yq = pyquat.Quaternion(y.q)
46
-
47
- dp = xq.inverse.rotate(y.p - x.p)
48
- dq = xq.inverse * yq
49
-
50
- return Pose(0, dp, dq.normalised.elements)
51
-
52
- @classmethod
53
- def interpolate(cls, a: Self, b: Self, t: float) -> Self:
54
- """
55
- Interpolates between two given poses, as a * t + b * (1 - t)
56
-
57
- :return: interpolated pose
58
- """
59
- ts = a.ts + (b.ts - a.ts) * t
60
- p = a.p + (b.p - a.p) * t
61
- q = pyquat.Quaternion.slerp(pyquat.Quaternion(a.q), pyquat.Quaternion(b.q), t)
62
- return Pose(ts, p, q.normalised.elements)
63
-
64
- def __init__(self, ts: float, p: np.ndarray, q: np.ndarray):
65
- """
66
- Represents a pose
67
-
68
- :param ts: timestamp
69
- :param p: position vector
70
- :param q: orientation quaternion
71
- """
72
- self.ts = ts
73
- self.p = p
74
- self.q = q
75
-
76
- def matrix(self) -> np.ndarray:
77
- """
78
- Returns the transformation matrix of this pose
79
-
80
- :return: transformation matrix
81
- """
82
- r = pyquat.Quaternion(self.q).rotation_matrix
83
- t = self.p[:, None]
84
- return np.block([[r, t], [np.zeros((1, 3), dtype=np.float64), np.ones((1, 1), dtype=np.float64)]])
85
-
86
- def translate(self, v: np.ndarray) -> np.ndarray:
87
- """
88
- Translate the given vector with the pose position
89
-
90
- :param v: the vector to be translated
91
-
92
- :return: translated vector
93
- """
94
- return self.p + v
95
-
96
- def rotate(self, v: np.ndarray) -> np.ndarray:
97
- """
98
- Rotates the given vector with the pose orientation
99
-
100
- :param v: the vector to be rotated
101
-
102
- :return: rotated vector
103
- """
104
- return pyquat.Quaternion(self.q).rotate(v)
105
-
106
- def __str__(self):
107
- return repr_data(self)
@@ -1,66 +0,0 @@
1
- import math
2
- import unittest
3
-
4
- import ddt
5
- import numpy as np
6
- import pyquaternion as pyquat
7
- from iker.common.utils.randutils import randomizer
8
-
9
- from plexus.common.pose import Pose
10
-
11
-
12
- @ddt.ddt
13
- class PoseTest(unittest.TestCase):
14
-
15
- @staticmethod
16
- def random_pose() -> Pose:
17
- q = pyquat.Quaternion(axis=randomizer().random_unit_vector(3),
18
- angle=math.pi * randomizer().next_float(0.0, 0.5))
19
- return Pose(0, np.array(randomizer().random_unit_vector(3)), q.normalised.elements)
20
-
21
- def assert_array_almost_equal(self, xs: np.ndarray, ys: np.ndarray, delta: float):
22
- self.assertEqual(len(xs), len(ys))
23
- for x, y in zip(xs, ys):
24
- self.assertAlmostEqual(x, y, delta=delta)
25
-
26
- def test_builtin_init(self):
27
- for _ in range(0, 10000):
28
- expect = PoseTest.random_pose()
29
- actual = Pose(0, expect.p, expect.q)
30
-
31
- self.assert_array_almost_equal(expect.p, actual.p, delta=1e-9)
32
- self.assert_array_almost_equal(expect.q, actual.q, delta=1e-9)
33
-
34
- def test_add_sub(self):
35
- for _ in range(0, 10000):
36
- pose = PoseTest.random_pose()
37
- delta = PoseTest.random_pose()
38
-
39
- result = Pose.sub(Pose.add(pose, delta), pose)
40
-
41
- self.assert_array_almost_equal(delta.p, result.p, delta=1e-9)
42
- self.assert_array_almost_equal(delta.q, result.q, delta=1e-9)
43
-
44
- def test_interpolate(self):
45
- for _ in range(0, 10000):
46
- t = randomizer().next_float()
47
-
48
- pose1 = PoseTest.random_pose()
49
- pose2 = PoseTest.random_pose()
50
-
51
- inter1 = Pose.interpolate(pose1, pose2, t)
52
- inter2 = Pose.interpolate(pose2, pose1, 1.0 - t)
53
-
54
- self.assert_array_almost_equal(inter1.p, inter2.p, delta=1e-9)
55
- self.assert_array_almost_equal(inter1.q, inter2.q, delta=1e-9)
56
-
57
- def test_matrix(self):
58
- for _ in range(0, 10000):
59
- point = np.array(randomizer().random_unit_vector(3))
60
-
61
- pose = PoseTest.random_pose()
62
-
63
- expect = pose.translate(pose.rotate(point))
64
- result = np.matmul(pose.matrix(), np.array([*point, 1.0])[:, None]).transpose()[0][:3]
65
-
66
- self.assert_array_almost_equal(expect, result, delta=1e-9)