plexus-python-common 1.0.31__py3-none-any.whl

plexus/common/proj.py

@@ -0,0 +1,305 @@
+from abc import ABC
+from abc import abstractmethod
+from typing import Annotated, Self
+
+import numpy as np
+import pydantic as pdt
+import pyproj
+from iker.common.utils.funcutils import singleton
+from iker.common.utils.strutils import parse_params_string, repr_data, str_conv
+
+from plexus.common.pose import Pose
+
+
+class Proj(ABC):
+    @classmethod
+    @abstractmethod
+    def typename(cls) -> str:
+        pass
+
+    def __init__(self, spec):
+        self.spec = spec
+
+    def __str__(self):
+        return self.to_string()
+
+    def method(self) -> str:
+        return self.typename()
+
+    @abstractmethod
+    def ellipsoid(self) -> str:
+        pass
+
+    @abstractmethod
+    def from_latlon(self, lat: float, lon: float) -> tuple[float, float]:
+        """
+        Performs forward projection from latitude/longitude to projection coordinate system
+
+        :param lat: latitude
+        :param lon: longitude
+
+        :return: tuple of x and y in projection coordinate system
+        """
+        pass
+
+    @abstractmethod
+    def to_latlon(self, x: float, y: float) -> tuple[float, float]:
+        """
+        Performs inverse projection from projection coordinate system to latitude/longitude
+
+        :param x: x in projection coordinate system
+        :param y: y in projection coordinate system
+
+        :return: tuple of latitude and longitude
+        """
+        pass
+
+    @abstractmethod
+    def to_string(self) -> str:
+        pass
+
+
+class EQDCProj(Proj):
+    """
+    Equidistant conic projection
+    Please refer to https://en.wikipedia.org/wiki/Equidistant_conic_projection
+    """
+
+    class Spec(pdt.BaseModel):
+        lat_1: Annotated[float, pdt.Strict(), pdt.AllowInfNan(False)]
+        lat_2: Annotated[float, pdt.Strict(), pdt.AllowInfNan(False)]
+        lon_0: Annotated[float, pdt.Strict(), pdt.AllowInfNan(False)] = 0.0
+
+    @classmethod
+    def typename(cls) -> str:
+        return "eqdc"
+
+    def __init__(self, **kwargs):
+        super(EQDCProj, self).__init__(EQDCProj.Spec(**kwargs))
+        self.proj = self.make_proj()
+
+    def ellipsoid(self) -> str:
+        return "WGS84"
+
+    def from_latlon(self, lat: float, lon: float) -> tuple[float, float]:
+        x, y = self.proj(lon, lat)
+        return x, y
+
+    def to_latlon(self, x: float, y: float) -> tuple[float, float]:
+        lon, lat = self.proj(x, y, inverse=True)
+        return lat, lon
+
+    def to_string(self) -> str:
+        return "{method}:lat_1={},lat_2={},lon_0={}".format(
+            self.spec.lat_1,
+            self.spec.lat_2,
+            self.spec.lon_0,
+            method=self.method(),
+        )
+
+    def make_proj(self):
+        return pyproj.Proj(
+            "+proj={method} +ellps={ellipsoid} +datum={ellipsoid} +units=m +no_defs +lat_1={} +lat_2={} +lon_0={}".format(
+                self.spec.lat_1,
+                self.spec.lat_2,
+                self.spec.lon_0,
+                method=self.method(),
+                ellipsoid=self.ellipsoid(),
+            )
+        )
+
+
+class UTMProj(Proj):
+    """
+    Universal Transverse Mercator coordinate system
+    Please refer to https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system
+    """
+
+    class Spec(pdt.BaseModel):
+        zone: Annotated[int, pdt.Strict()]
+        south: Annotated[bool, pdt.Strict()] = False
+
+    @classmethod
+    def typename(cls) -> str:
+        return "utm"
+
+    def __init__(self, **kwargs):
+        super(UTMProj, self).__init__(UTMProj.Spec(**kwargs))
+        self.proj = self.make_proj()
+
+    def ellipsoid(self) -> str:
+        return "WGS84"
+
+    def from_latlon(self, lat: float, lon: float) -> tuple[float, float]:
+        x, y = self.proj(lon, lat)
+        return x, y
+
+    def to_latlon(self, x: float, y: float) -> tuple[float, float]:
+        lon, lat = self.proj(x, y, inverse=True)
+        return lat, lon
+
+    def to_string(self) -> str:
+        return "{method}:zone={},south={}".format(self.spec.zone, self.spec.south, method=self.method())
+
+    def make_proj(self):
+        return pyproj.Proj(
+            "+proj={method} +ellps={ellipsoid} +datum={ellipsoid} +units=m +no_defs +zone={} {}".format(
+                self.spec.zone,
+                "+south" if self.spec.south else "",
+                method=self.method(),
+                ellipsoid=self.ellipsoid(),
+            )
+        )
+
+
+class WebMercProj(Proj):
+    """
+    Web Mercator projection
+    Please refer to https://en.wikipedia.org/wiki/Web_Mercator_projection
+    """
+
+    class Spec(pdt.BaseModel):
+        lon_0: Annotated[float, pdt.Strict(), pdt.AllowInfNan(False)] = 0.0
+
+    @classmethod
+    def typename(cls) -> str:
+        return "webmerc"
+
+    def __init__(self, **kwargs):
+        super(WebMercProj, self).__init__(WebMercProj.Spec(**kwargs))
+        self.proj = self.make_proj()
+
+    def ellipsoid(self) -> str:
+        return "WGS84"
+
+    def from_latlon(self, lat: float, lon: float) -> tuple[float, float]:
+        x, y = self.proj(lon, lat)
+        return x, y
+
+    def to_latlon(self, x: float, y: float) -> tuple[float, float]:
+        lon, lat = self.proj(x, y, inverse=True)
+        return lat, lon
+
+    def to_string(self) -> str:
+        return "{method}:lon_0={}".format(self.spec.lon_0, method=self.method())
+
+    def make_proj(self):
+        return pyproj.Proj(
+            "+proj={method} +ellps={ellipsoid} +datum={ellipsoid} +units=m +no_defs +lon_0={}".format(
+                self.spec.lon_0,
+                method=self.method(),
+                ellipsoid=self.ellipsoid(),
+            ),
+        )
+
+
+@singleton
+def default_proj() -> Proj:
+    return make_proj("eqdc:lat_1=30.0,lat_2=50.0")
+
+
+def make_proj(spec_str: str) -> Proj | None:
+    """
+    Constructs cartographic projection from the given name and arguments
+
+    :param spec_str: projection spec string
+
+    :return: projection
+    """
+
+    if spec_str is None:
+        return None
+
+    match spec_str.split(":", maxsplit=1):
+        case [name]:
+            kwargs = {}
+        case [name, args]:
+            kwargs = parse_params_string(args, str_parser=str_conv)
+        case _:
+            raise ValueError("bad spec string")
+
+    if name.lower() == UTMProj.typename():
+        return UTMProj(**kwargs)
+    if name.lower() == EQDCProj.typename():
+        return EQDCProj(**kwargs)
+    if name.lower() == WebMercProj.typename():
+        return WebMercProj(**kwargs)
+
+    return None
+
+
+class Coord(object):
+    @classmethod
+    def from_pose(cls, pose: Pose, proj: Proj | None) -> Self:
+        """
+        Creates a coord from given pose
+
+        :param pose: pose based on which the coord is created
+        :param proj: cartographic projection
+
+        :return: generated coord
+        """
+        return cls.from_xy(pose.p[0], pose.p[1], pose.p[2], proj, pose.ts)
+
+    @classmethod
+    def from_latlon(cls, lat: float, lon: float, ele: float, proj: Proj | None, ts: float = 0) -> Self:
+        """
+        Creates a coord from latitude/longitude
+
+        :param lat: latitude
+        :param lon: longitude
+        :param ele: elevation
+        :param proj: cartographic projection
+        :param ts: timestamp
+
+        :return: generated coord
+        """
+        if proj is None:
+            return Coord(ts, lat, lon, ele, 0.0, 0.0, pdt.BaseModel())
+
+        x, y = proj.from_latlon(lat, lon)
+        return Coord(ts, lat, lon, ele, x, y, proj.spec)
+
+    @classmethod
+    def from_xy(cls, x: float, y: float, ele: float, proj: Proj | None, ts: float = 0) -> Self:
+        """
+        Creates a coord from coordinate in cartographic projection
+
+        :param x: x coordinate in cartographic projection
+        :param y: y coordinate in cartographic projection
+        :param ele: elevation
+        :param proj: cartographic projection
+        :param ts: timestamp
+
+        :return: a coord
+        """
+        if proj is None:
+            return Coord(ts, 0.0, 0.0, ele, x, y, pdt.BaseModel())
+
+        lat, lon = proj.to_latlon(x, y)
+        return Coord(ts, lat, lon, ele, x, y, proj.spec)
+
+    def __init__(self, ts: float, lat: float, lon: float, ele: float, x: float, y: float, spec: pdt.BaseModel):
+        """
+        Represents a coordinate
+
+        :param ts: timestamp
+        :param lat: latitude
+        :param lon: longitude
+        :param ele: elevation
+        :param x: x in projection coordinate system
+        :param y: y in projection coordinate system
+        :param spec: projection specification
+        """
+        self.ts = ts
+        self.lat = lat
+        self.lon = lon
+        self.x = x
+        self.y = y
+        self.spec = spec
+        self.ele = ele
+        self.xy = np.array([x, y], dtype=np.float64)
+        self.xyz = np.array([x, y, ele], dtype=np.float64)
+
+    def __str__(self):
+        return repr_data(self)

plexus/common/utils/apiutils.py

@@ -0,0 +1,31 @@
+from contextlib import AbstractContextManager
+
+import sqlalchemy.orm as sa_orm
+from fastapi import HTTPException
+
+__all__ = [
+    "managed_db_session",
+]
+
+
+class DBSessionExceptionManager(AbstractContextManager):
+    def __init__(self, db: sa_orm.Session, commit_on_exit: bool):
+        self.db = db
+        self.commit_on_exit = commit_on_exit
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_value, traceback):
+        if isinstance(exc_type, HTTPException):
+            return False  # Propagate HTTPException
+        if exc_type is not None:
+            self.db.rollback()  # Raise a new HTTPException (or any other exception type)
+            raise HTTPException(status_code=500, detail=str(exc_value)) from exc_value
+        if self.commit_on_exit:
+            self.db.commit()
+        return True
+
+
+def managed_db_session(db: sa_orm.Session, commit_on_exit: bool = True) -> DBSessionExceptionManager:
+    return DBSessionExceptionManager(db, commit_on_exit)

plexus/common/utils/bagutils.py

@@ -0,0 +1,215 @@
+import os
+from collections.abc import Generator
+from contextlib import AbstractContextManager
+from typing import Literal
+
+import sqlalchemy as sa
+import sqlalchemy.orm as sa_orm
+
+__all__ = [
+    "SerializationFormat",
+    "BagTopic",
+    "BagMessage",
+    "BagSchema",
+    "BagMessageDefinition",
+    "BagMetadata",
+    "BagReader",
+    "BagWriter",
+    "bag_reader",
+    "bag_writer",
+]
+
+SerializationFormat = Literal["cdr", "cdr2", "json", "yaml"]
+
+default_bag_db_file = "bag.db"
+
+
+class BagBase(sa_orm.DeclarativeBase):
+    pass
+
+
+class BagSchema(BagBase):
+    __tablename__ = "schema"
+
+    schema_version: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, primary_key=True)
+    ros_distro: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+
+
+class BagMetadata(BagBase):
+    __tablename__ = "metadata"
+
+    id: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, primary_key=True, autoincrement=True)
+    metadata_version: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, nullable=False)
+    metadata_text: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+
+
+class BagTopic(BagBase):
+    __tablename__ = "topics"
+
+    id: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, primary_key=True, autoincrement=True)
+    name: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    type: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    serialization_format: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    type_description_hash: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+
+    messages: sa_orm.Mapped[list["BagMessage"]] = sa_orm.relationship("BagMessage", back_populates="topic")
+
+
+class BagMessageDefinition(BagBase):
+    __tablename__ = "message_definitions"
+
+    id: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, primary_key=True, autoincrement=True)
+    topic_type: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    encoding: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    encoded_message_definition: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+    type_description_hash: sa_orm.Mapped[str] = sa_orm.mapped_column(sa.String, nullable=False)
+
+
+class BagMessage(BagBase):
+    __tablename__ = "messages"
+
+    id: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, primary_key=True, autoincrement=True)
+    topic_id: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, sa.ForeignKey("topics.id"), nullable=False)
+    timestamp: sa_orm.Mapped[int] = sa_orm.mapped_column(sa.Integer, nullable=False)
+    data: sa_orm.Mapped[bytes] = sa_orm.mapped_column(sa.LargeBinary, nullable=False)
+
+    topic: sa_orm.Mapped["BagTopic"] = sa_orm.relationship("BagTopic", back_populates="messages")
+
+
+class BagReader(AbstractContextManager):
+    def __init__(self, db_dir: str, db_file: str = default_bag_db_file):
+        self.db_path = os.path.join(db_dir, db_file)
+        if not os.path.exists(self.db_path):
+            raise FileNotFoundError(f"could not find SQLite DB at '{db_dir}'")
+
+        self.engine = sa.create_engine(f"sqlite:///{self.db_path}")
+        self.session = sa_orm.Session(self.engine)
+        self.topic_map = None
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    def close(self):
+        self.session.close()
+        self.engine.dispose()
+
+    def get_topics(self) -> dict[str, BagTopic]:
+        if self.topic_map is None:
+            stmt = sa.select(BagTopic)
+            self.topic_map = {db_topic.name: db_topic for db_topic in self.session.scalars(stmt).all()}
+        return self.topic_map
+
+    def iter_messages(
+        self,
+        topic_names: list[str] | None = None,
+        begin_timestamp: int | None = None,
+        end_timestamp: int | None = None,
+    ) -> Generator[BagMessage, None, None]:
+        stmt = sa.select(BagMessage)
+        if topic_names is not None:
+            topic_ids = [db_topic.id for db_topic in self.get_topics().values() if db_topic.name in topic_names]
+            stmt = stmt.where(BagMessage.topic_id.in_(topic_ids))
+        if begin_timestamp is not None:
+            stmt = stmt.where(BagMessage.timestamp >= begin_timestamp)
+        if end_timestamp is not None:
+            stmt = stmt.where(BagMessage.timestamp <= end_timestamp)
+        for db_message in self.session.scalars(stmt.order_by(BagMessage.timestamp)):
+            yield db_message
+
+
+class BagWriter(AbstractContextManager):
+    def __init__(self, db_dir: str, db_file: str = default_bag_db_file):
+        self.db_path = os.path.join(db_dir, db_file)
+        if os.path.exists(self.db_path):
+            os.remove(self.db_path)
+
+        self.engine = sa.create_engine(f"sqlite:///{self.db_path}")
+        self.session = sa_orm.Session(self.engine)
+        self.topic_map = {}
+
+        BagBase.metadata.create_all(self.engine)
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    def close(self) -> None:
+        self.session.commit()
+        self.session.close()
+        self.engine.dispose()
+
+    def get_or_create_topic(
+        self,
+        topic_name: str,
+        topic_type: str,
+        *,
+        serialization_format: SerializationFormat = "cdr",
+    ) -> BagTopic:
+        if topic_name in self.topic_map:
+            return self.topic_map[topic_name]
+
+        db_topic = self.session.query(BagTopic).filter(BagTopic.name == topic_name).first()
+        if db_topic is None:
+            db_topic = BagTopic(name=topic_name,
+                                type=topic_type,
+                                serialization_format=serialization_format,
+                                type_description_hash="")
+            self.session.add(db_topic)
+            self.session.flush()
+
+        self.topic_map[topic_name] = db_topic
+        self.session.commit()
+
+        return db_topic
+
+    def write_message(
+        self,
+        topic_name: str,
+        timestamp: int,
+        data: bytes,
+        topic_type: str = None,
+        *,
+        serialization_format: SerializationFormat = "cdr",
+    ) -> BagMessage:
+        if topic_name not in self.topic_map:
+            if topic_type is None:
+                raise ValueError(f"missing topic type to create topic '{topic_name}' dynamically")
+
+            self.get_or_create_topic(topic_name, topic_type, serialization_format=serialization_format)
+
+        db_topic = self.topic_map[topic_name]
+        db_message = BagMessage(topic_id=db_topic.id, timestamp=timestamp, data=data)
+
+        self.session.add(db_message)
+        self.session.flush()
+
+        return db_message
+
+
+def bag_reader(db_dir: str, db_file: str = default_bag_db_file) -> BagReader:
+    """
+    Creates a BagReader instance to read messages from a ROS2 bag file.
+
+    :param db_dir: directory containing the SQLite DB file.
+    :param db_file: name of the SQLite DB file.
+    :return: BagReader instance.
+    """
+    return BagReader(db_dir, db_file)
+
+
+def bag_writer(db_dir: str, db_file: str = default_bag_db_file) -> BagWriter:
+    """
+    Creates a BagWriter instance to write messages to a ROS2 bag file.
+
+    :param db_dir: directory to store the SQLite DB file.
+    :param db_file: name of the SQLite DB file.
+    :return: BagWriter instance.
+    """
+    if not os.path.exists(db_dir):
+        os.makedirs(db_dir, exist_ok=True)
+    return BagWriter(db_dir, db_file)

plexus/common/utils/config.py

@@ -0,0 +1,61 @@
+from iker.common.utils import logger
+from iker.common.utils.config import Config
+from iker.common.utils.funcutils import memorized
+from iker.common.utils.shutils import expanded_path
+
+
+@memorized()
+def config(config_path: str = "") -> Config:
+    default_items: list[tuple[str, str, str]] = [
+        ("plexus", "logging.level", "INFO"),
+        ("plexus", "logging.format", "%(asctime)s [%(levelname)s] %(name)s: %(message)s"),
+    ]
+
+    instance = Config(config_path or expanded_path("~/.plexus.cfg"))
+    instance.restore()
+    instance.update(default_items, overwrite=False)
+
+    return instance
+
+
+def config_print_or_set(config: Config, section: str, key: str, value: str):
+    if value is not None:
+        if section is None or key is None:
+            raise ValueError("cannot specify value without section and key")
+
+        old_value = config.get(section, key)
+        config.set(section, key, value)
+        config.persist()
+
+        print(f"Configuration file '{config.config_path}'", )
+        print(f"Section <{section}>")
+        print(f"  {key} = {old_value} -> {value}")
+
+    else:
+        if section is None and key is None:
+            print(f"Configuration file '{config.config_path}'", )
+            for section in config.config_parser.sections():
+                print(f"Section <{section}>")
+                for key, value in config.config_parser.items(section):
+                    print(f"  {key} = {value}")
+
+        elif section is not None and key is None:
+            if not config.has_section(section):
+                logger.warning("Configuration section <%s> not found", section)
+                return
+            print(f"Configuration file '{config.config_path}'", )
+            print(f"Section <{section}>")
+            for key, value in config.config_parser.items(section):
+                print(f"  {key} = {value}")
+
+        elif section is not None and key is not None:
+            value = config.get(section, key)
+            if value is None:
+                logger.warning("Configuration section <%s> key <%s> not found", section, key)
+                return
+            print(f"Configuration file '{config.config_path}'", )
+            print(f"Section <{section}>")
+            print(f"  {key} = {value}")
+
+        else:
+            raise ValueError("cannot specify key without section")